#include <thread>
#include <iostream>
#include <fstream>
#include <sys/shm.h>
#include <sys/sem.h>
#include <malloc.h>
#include "zucmodule_cmd.h"
#include "pyport.h"
#include "zucmodule_exceptions.h"
#include "rcs.hh"
#include "rcs_print.hh"
#include "nml_intf/zuc_nml.hh"
#include "timer.hh"
#include "motion/semintf.h"
#include "kinintf.h"
#include "joint_space_smooth.h"
#include "jkutil/iniparser.h"
#include "motion/torque_control_queue.h"

#define ZUC_COMMAND_TIMEOUT 15.0  // how long to wait until timeout
#define ZUC_COMMAND_DELAY 0.001   // how long to sleep between checks

struct pyCommandChannel
{
    PyObject_HEAD RCS_CMD_CHANNEL* c;
    RCS_STAT_CHANNEL* s;
    int serial;
};

static int zucWaitCommandComplete(pyCommandChannel* s, double timeout)
{
    double start = etime();

    do {
        double now = etime();
        if (s->s->peek() == ZUC_STAT_TYPE)
        {
            ZUC_STAT* stat = (ZUC_STAT*)s->s->get_address();
            int serial_diff = stat->echo_serial_number - s->serial;
            if (serial_diff > 0)
            {
                return RCS_DONE;
            }
            if (serial_diff == 0 && (stat->status == RCS_DONE || stat->status == RCS_ERROR))
            {
                return stat->status;
            }
        }
        esleep(fmin(timeout - (now - start), ZUC_COMMAND_DELAY));
    } while (etime() - start < timeout);
    return -1;
}

static int zucSendCommand(pyCommandChannel* s, RCS_CMD_MSG& cmd)
{
    if (s->c->write(&cmd))
    {
        return -1;
    }
    s->serial = cmd.serial_number;

    double start = etime();
    while (etime() - start < ZUC_COMMAND_TIMEOUT)
    {
        ZUC_STAT* stat = (ZUC_STAT*)s->s->get_address();
        int serial_diff = stat->echo_serial_number - s->serial;
        if (s->s->peek() == ZUC_STAT_TYPE && serial_diff >= 0)
        {
            return 0;
        }
        esleep(ZUC_COMMAND_DELAY);
    }
    return -1;
}

static int Command_init(pyCommandChannel* self, PyObject* a, PyObject* k)
{
    const char* file = ZUC_DEFAULT_NMLFILE;
    if (file == NULL)
        return -1;

    RCS_CMD_CHANNEL* c = new RCS_CMD_CHANNEL(zucFormat, "zucCommand", "xzuc", file);
    if (!c)
    {
        PyErr_Format(Exception_GetBaseError(), "new RCS_CMD_CHANNEL failed");
        return -1;
    }
    RCS_STAT_CHANNEL* s = new RCS_STAT_CHANNEL(zucFormat, "zucStatus", "xzuc", file);
    if (!c)
    {
        delete s;
        PyErr_Format(Exception_GetBaseError(), "new RCS_STAT_CHANNEL failed");
        return -1;
    }

    self->s = s;
    self->c = c;
    return 0;
}

static void Command_dealloc(PyObject* self)
{
    delete ((pyCommandChannel*)self)->c;
    PyObject_Del(self);
}

static PyObject* block_delete(pyCommandChannel* s, PyObject* o)
{
    int t;
    ZUC_TASK_PLAN_SET_BLOCK_DELETE m;

    if (!PyArg_ParseTuple(o, "i", &t))
        return NULL;
    m.state = t;

    zucSendCommand(s, m);

    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* optional_stop(pyCommandChannel* s, PyObject* o)
{
    int t;
    ZUC_TASK_PLAN_SET_OPTIONAL_STOP m;

    if (!PyArg_ParseTuple(o, "i", &t))
        return NULL;
    m.state = t;

    zucSendCommand(s, m);

    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* mode(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_SET_MODE m;
    if (!PyArg_ParseTuple(o, "i", &m.mode))
        return NULL;
    switch (m.mode)
    {
    case ZUC_TASK_MODE_MDI:
    case ZUC_TASK_MODE_MANUAL:
    case ZUC_TASK_MODE_AUTO:
    case ZUC_TASK_MODE_DRAG:
        break;
    default:
        PyErr_Format(PyExc_ValueError, "Mode should be MODE_MDI, MODE_MANUAL, or MODE_AUTO");
        return NULL;
    }
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* setOperationMode(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_SET_OPERATION_MODE m;
    if (!PyArg_ParseTuple(o, "i", &m.opmode))
        return NULL;
    switch (m.opmode)
    {
    case ZUC_TASK_OPERATION_MODE_MANUAL:
    case ZUC_TASK_OPERATION_MODE_AUTO:
        break;
    default:
        PyErr_Format(PyExc_ValueError, "Mode should be MODE_MANUAL or MODE_AUTO");
        return NULL;
    }
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* task_plan_synch(pyCommandChannel* s)
{
    printf(" [task_plan_synch]: \n ");
    ZUC_TASK_PLAN_SYNCH synch;
    zucSendCommand(s, synch);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* maxvel(pyCommandChannel* s, PyObject* o)
{
    ZUC_TRAJ_SET_MAX_VELOCITY m;
    if (!PyArg_ParseTuple(o, "d", &m.velocity))
        return NULL;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* feedrate(pyCommandChannel* s, PyObject* o)
{
    ZUC_TRAJ_SET_SCALE m;
    if (!PyArg_ParseTuple(o, "d", &m.scale))
        return NULL;
    zucSendCommand(s, m);
    // s->c->write(&m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* rapidrate(pyCommandChannel* s, PyObject* o)
{
    ZUC_TRAJ_SET_RAPID_SCALE m;
    if (!PyArg_ParseTuple(o, "d", &m.scale))
        return NULL;
    zucSendCommand(s, m);
    // s->c->write(&m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* spindle(pyCommandChannel* s, PyObject* o) { return Py_None; }

static PyObject* mdi(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_PLAN_EXECUTE m;
    char* cmd;
    Py_ssize_t len;
    if (!PyArg_ParseTuple(o, "s#", &cmd, &len))
        return NULL;
    if (unsigned(len) > sizeof(m.command) - 1)
    {
        PyErr_Format(PyExc_ValueError, "MDI commands limited to %zu characters", sizeof(m.command) - 1);
        return NULL;
    }
    strcpy(m.command, cmd);
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_user_frame(pyCommandChannel* s, PyObject* o)
{
    ZUC_TRAJ_SET_ROBOT_USER_FRAME m;
    if (!PyArg_ParseTuple(o,
                          "dddddd",
                          &m.robotUserFrame.tran.x,
                          &m.robotUserFrame.tran.y,
                          &m.robotUserFrame.tran.z,
                          &m.robotUserFrame.a,
                          &m.robotUserFrame.b,
                          &m.robotUserFrame.c))
        return NULL;

    m.robotUserFrame.u = 0.0;
    m.robotUserFrame.v = 0.0;
    m.robotUserFrame.w = 0.0;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_installation_angle(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_MUTI_USER_OFFSET m;
    m.method = 0;
    double no_use[4] = {};  // 四元数，暂时不用
    if (!PyArg_ParseTuple(o,
                          "(dddd)(ddd)|(ddd)",
                          &no_use[0],
                          &no_use[1],
                          &no_use[2],
                          &no_use[3],
                          &m.robotUserOffset.a,
                          &m.robotUserOffset.b,
                          &m.robotUserOffset.c,
                          &m.robotUserOffset.tran.x,
                          &m.robotUserOffset.tran.y,
                          &m.robotUserOffset.tran.z))
        return NULL;
    // TODO 后续换接口
    char name1[] = "BASE_0";
    strcpy(m.name, name1);
    m.id = 16;
    zucSendCommand(s, m);
    char name2[] = "BASE_1";
    strcpy(m.name, name2);
    m.id = 17;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_muti_tool_offsets(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_MUTI_TOOL_OFFSET m;
    char* name;
    Py_ssize_t len;
    if (!PyArg_ParseTuple(o,
                          "(dddddd)iis#",
                          &m.robotToolOffset.tran.x,
                          &m.robotToolOffset.tran.y,
                          &m.robotToolOffset.tran.z,
                          &m.robotToolOffset.a,
                          &m.robotToolOffset.b,
                          &m.robotToolOffset.c,
                          &m.method,
                          &m.id,
                          &name,
                          &len))
    {
        return NULL;
    }
    if (unsigned(len) > sizeof(m.name) - 1)
    {
        PyErr_Format(PyExc_ValueError, "Tool name limited to %zu characters", sizeof(m.name) - 1);
        return NULL;
    }
    strcpy(m.name, name);
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_muti_user_offsets(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_MUTI_USER_OFFSET m;
    char* name;
    Py_ssize_t len;
    if (!PyArg_ParseTuple(o,
                          "(dddddd)iis#",
                          &m.robotUserOffset.tran.x,
                          &m.robotUserOffset.tran.y,
                          &m.robotUserOffset.tran.z,
                          &m.robotUserOffset.a,
                          &m.robotUserOffset.b,
                          &m.robotUserOffset.c,
                          &m.method,
                          &m.id,
                          &name,
                          &len))
    {
        return NULL;
    }
    if (unsigned(len) > sizeof(m.name) - 1)
    {
        PyErr_Format(PyExc_ValueError, "User name limited to %zu characters", sizeof(m.name) - 1);
        return NULL;
    }
    strcpy(m.name, name);
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_safe_zone(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_SAFE_ZONE z;
    char* name;
    Py_ssize_t len;
    int id;
    int enable;
    int safeZoneMode;
    double safeDist;
    double pointValue[5][6];
    int coordinateSystemTransformation = 0;  // 是否要对安全平面配置的笛卡尔坐标进行转换，转换为相对世界坐标系的笛卡尔坐标
    if (!PyArg_ParseTuple(o,
                          "iiiiii((dddddd)(dddddd)(dddddd)(dddddd))idis#|i",
                          &z.openEnable,
                          &z.autoEnable,
                          &safeZoneMode,
                          &z.zone.safe,
                          &z.zone.elbow_pos_limit_enable,
                          &z.zone.wrist_pos_limit_enable,
                          &pointValue[0][0],
                          &pointValue[0][1],
                          &pointValue[0][2],
                          &pointValue[0][3],
                          &pointValue[0][4],
                          &pointValue[0][5],
                          &pointValue[1][0],
                          &pointValue[1][1],
                          &pointValue[1][2],
                          &pointValue[1][3],
                          &pointValue[1][4],
                          &pointValue[1][5],
                          &pointValue[2][0],
                          &pointValue[2][1],
                          &pointValue[2][2],
                          &pointValue[2][3],
                          &pointValue[2][4],
                          &pointValue[2][5],
                          &pointValue[3][0],
                          &pointValue[3][1],
                          &pointValue[3][2],
                          &pointValue[3][3],
                          &pointValue[3][4],
                          &pointValue[3][5],
                          &enable,
                          &safeDist,
                          &id,
                          &name,
                          &len,
                          &coordinateSystemTransformation))
    {
        return NULL;
    }

    for (int i = 0; i < 6; i++) { z.zone.zone.plane[i].modify = 0; }
    z.zone.safeZoneMode = safeZoneMode;
    z.coordinateSystemTransformation = coordinateSystemTransformation;
    if (id >= 0 && id <= 5)
    {
        for (int i = 0; i < 3; i++)
        {
            z.zone.zone.plane[id].planePoints[i].point.x = pointValue[i][0];
            z.zone.zone.plane[id].planePoints[i].point.y = pointValue[i][1];
            z.zone.zone.plane[id].planePoints[i].point.z = pointValue[i][2];
            z.zone.zone.plane[id].planePoints[i].rpy.y = pointValue[i][3];
            z.zone.zone.plane[id].planePoints[i].rpy.p = pointValue[i][4];
            z.zone.zone.plane[id].planePoints[i].rpy.r = pointValue[i][5];
        }
        z.zone.zone.plane[id].safePoint.point.x = pointValue[3][0];
        z.zone.zone.plane[id].safePoint.point.y = pointValue[3][1];
        z.zone.zone.plane[id].safePoint.point.z = pointValue[3][2];
        z.zone.zone.plane[id].safePoint.rpy.y = pointValue[3][3];
        z.zone.zone.plane[id].safePoint.rpy.p = pointValue[3][4];
        z.zone.zone.plane[id].safePoint.rpy.r = pointValue[3][5];

        z.zone.zone.plane[id].enable = enable;
        z.zone.zone.plane[id].safeD = safeDist;
        z.zone.zone.plane[id].modify = 1;
        if (unsigned(len) > sizeof(z.zone.zone.plane[id].name) - 1)
        {
            PyErr_Format(PyExc_ValueError, "User name limited to %zu characters", sizeof(z.zone.zone.plane[id].name) - 1);
            return NULL;
        }
        strcpy(z.zone.zone.plane[id].name, name);
    }

    zucSendCommand(s, z);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* world_cartesian_point_convert_to_user(pyCommandChannel* s, PyObject* o)
{
    CartesianPos cartesianPos;
    if (!PyArg_ParseTuple(
            o, "(dddddd)", &cartesianPos.point.x, &cartesianPos.point.y, &cartesianPos.point.z, &cartesianPos.rpy.y, &cartesianPos.rpy.p, &cartesianPos.rpy.r))
    {
        return NULL;
    }
    s->s->peek();
    ZUC_STAT* stat = (ZUC_STAT*)s->s->get_address();
    convertWorldPointToUser(&cartesianPos, &stat->userOffset[stat->motion.currentUserId], &cartesianPos);
    PyObject* point = PyTuple_New(CART_DIM);
    PyTuple_SetItem(point, 0, PyFloat_FromDouble(cartesianPos.point.x));
    PyTuple_SetItem(point, 1, PyFloat_FromDouble(cartesianPos.point.y));
    PyTuple_SetItem(point, 2, PyFloat_FromDouble(cartesianPos.point.z));
    PyTuple_SetItem(point, 3, PyFloat_FromDouble(cartesianPos.rpy.y));
    PyTuple_SetItem(point, 4, PyFloat_FromDouble(cartesianPos.rpy.p));
    PyTuple_SetItem(point, 5, PyFloat_FromDouble(cartesianPos.rpy.r));
    return point;
}

static PyObject* user_cartesian_point_convert_to_world(pyCommandChannel* s, PyObject* o)
{
    CartesianPos cartesianPos;
    if (!PyArg_ParseTuple(
            o, "(dddddd)", &cartesianPos.point.x, &cartesianPos.point.y, &cartesianPos.point.z, &cartesianPos.rpy.y, &cartesianPos.rpy.p, &cartesianPos.rpy.r))
    {
        return NULL;
    }
    s->s->peek();
    ZUC_STAT* stat = (ZUC_STAT*)s->s->get_address();
    printf("ID :%d\n", stat->motion.currentUserId);
    printf("offset x = %f  y = %f  z = %f\n",
           stat->userOffset[stat->motion.currentUserId].tran.x,
           stat->userOffset[stat->motion.currentUserId].tran.y,
           stat->userOffset[stat->motion.currentUserId].tran.z);
    convertUserPointToWorld(&cartesianPos, &stat->userOffset[stat->motion.currentUserId], &cartesianPos);
    PyObject* point = PyTuple_New(CART_DIM);
    PyTuple_SetItem(point, 0, PyFloat_FromDouble(cartesianPos.point.x));
    PyTuple_SetItem(point, 1, PyFloat_FromDouble(cartesianPos.point.y));
    PyTuple_SetItem(point, 2, PyFloat_FromDouble(cartesianPos.point.z));
    PyTuple_SetItem(point, 3, PyFloat_FromDouble(cartesianPos.rpy.y));
    PyTuple_SetItem(point, 4, PyFloat_FromDouble(cartesianPos.rpy.p));
    PyTuple_SetItem(point, 5, PyFloat_FromDouble(cartesianPos.rpy.r));
    return point;
}

static PyObject* set_planner_jerk(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_PLANNER_JERK z;

    if (!PyArg_ParseTuple(o, "dd", &z.joint_jerk, &z.carte_jerk))
    {
        return NULL;
    }
    zucSendCommand(s, z);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_safe_attitude(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_SAFE_ATTITUDE z;

    if (!PyArg_ParseTuple(o,
                          "(ddd)ddddiii",
                          &z.conicalRpy.y,
                          &z.conicalRpy.p,
                          &z.conicalRpy.r,
                          &z.tilt,
                          &z.pan,
                          &z.deviation,
                          &z.deviationwarn,
                          &z.openEnable,
                          &z.autoEnable,
                          &z.tipChoice))
    {
        return NULL;
    }
    zucSendCommand(s, z);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_network_exception_handle(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_NET_EXCEPTION_HANDLE m;
    if (!PyArg_ParseTuple(o, "fi", &m.timeLimit, &m.motType))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

// int grpc_errcode;
// char grpc_errmsg[512];
static PyObject* set_grpc_errcode(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_GRPC_ERRCODE cmd;
    long int errtype;
    long int errcode;
    int repeatflag;
    char* errmsg;
    int saveFlag = 0;
    /**
     * errType: 错误类型，1：Info、2：Warnning、3：Error
     * errCode: 错误码
     * errMsg：错误描述信息
     * repeatFlag： 报错是否被检出，GRPC、Addon、SDK、OTA各占一个比特位，检出时清除对应标记位
     * saveFlag：该报错是否写入控制器errlog日志（若写入可在APP日志页面查看）
    */
    if (!PyArg_ParseTuple(o, "iisii", &errtype, &errcode, &errmsg, &repeatflag, &saveFlag))
        return NULL;
    cmd.grpc_errtype = errtype;
    cmd.grpc_errcode = errcode;
    strncpy(cmd.grpc_errmsg, errmsg, LINELEN);
    cmd.repeatflag = repeatflag;
    cmd.saveFlag = saveFlag;  //是否写入errorlog文件
    zucSendCommand(s, cmd);

    Py_INCREF(Py_None);
    return Py_None;
}

// static PyObject *get_grpc_errcode()
// {
//     PyObject* res = PyTuple_New(2);
//     PyTuple_SetItem(res, 0, PyLong_FromLong(grpc_errcode));
//     PyObject* result = Py_BuildValue("s", grpc_errmsg);
//     PyTuple_SetItem(res, 1, result);
//     return res;
// }

// int backup_errcode;
// char errcode_msg[512];
// static PyObject* set_backup_errcode(pyCommandChannel* s, PyObject* o)
// {
//     char* err;
//     if (!PyArg_ParseTuple(o, "is", &backup_errcode, &err))
//         return NULL;
//     strcpy(errcode_msg, err);

//     Py_INCREF(Py_None);
//     return Py_None;
// }

// static PyObject* get_backup_errcode()
// {
//     PyObject* res = PyTuple_New(2);
//     PyTuple_SetItem(res, 0, PyLong_FromLong(backup_errcode));
//     PyObject* result = Py_BuildValue("s", errcode_msg);
//     PyTuple_SetItem(res, 1, result);
//     return res;
// }

static PyObject* set_user_id(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_USER_ID m;
    if (!PyArg_ParseTuple(o, "ii", &m.robot_id, &m.id))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_robot_safety_settings(pyCommandChannel* s, PyObject* o)
{
    ZUC_ROBOT_SAFETY r;
    int cus, par;
    double mom, tcp, pow, stDist, stTime;
    PyObject* res = Py_None;
    if (!PyArg_ParseTuple(o, "iiddddd", &cus, &par, &mom, &tcp, &pow, &stDist, &stTime))
    {
        return NULL;
    }
    r.custome = cus;
    r.paramType = par;
    r.momentumLimit = mom;
    r.tcpVelLimit = tcp;
    r.robotPowerLimit = pow;
    r.stoppingDistance = stDist;
    r.stoppingTime = stTime;

    if (PyTuple_Size(o) < 2)
    {
        PyErr_Format(PyExc_TypeError, "set_robot_safety_settings(...) takes at least 2 arguments (%lu given)", (unsigned long)PyTuple_Size(o));
        return NULL;
    }

    r.type = 0;
    zucSendCommand(s, r);
    esleep(0.1);

    ZUC_STAT* stat = (ZUC_STAT*)s->s->get_address();
    if (stat->usrOpeErrCode != 0)
    {
        res = Py_BuildValue("i", stat->usrOpeErrCode);
        return res;
    }

    res = Py_BuildValue("i", 0);
    return res;
}

static PyObject* set_7_dof_inv_inform(pyCommandChannel* s, PyObject* o)
{
    Robot_7_DOF_INV_INFORM r;
    int inv_flag;  // 0: 臂角固定且根据当前机器人关节角状态计算 1: 臂角固定且根据输入arm_angle计算 2: 臂型角随动，根据当前臂型角为起点
    double arm_angle;
    if (!PyArg_ParseTuple(o, "id", &inv_flag, &arm_angle))
    {
        return NULL;
    }
    r.inv_flag = inv_flag;
    r.arm_angle = arm_angle;
    // kin_set_robot_Arm_angle_fixed(arm_angle);
    // kin_set_dof_7_inv_type(inv_flag);
    // printf(" inv_type %d arm_angle %f\n",inv_flag, arm_angle);
    zucSendCommand(s, r);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* get_robot_safety_settings(pyCommandChannel* s, PyObject* o)
{
    ZUC_ROBOT_SAFETY robotSafetySettings;
    robotSafetySettings.type = 1;
    zucSendCommand(s, robotSafetySettings);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_reduce_safety_settings(pyCommandChannel* s, PyObject* o)
{
    ZUC_REDUCE_SAFETY r;
    double tcp_speed, elbow_speed, momentum_limit, power_limit, stop_time, stop_dist;
    if (!PyArg_ParseTuple(o, "dddddd", &tcp_speed, &elbow_speed, &momentum_limit, &power_limit, &stop_time, &stop_dist))
    {
        return NULL;
    }
    r.tcp_speed = tcp_speed;
    r.elbow_speed = elbow_speed;
    r.momentum_limit = momentum_limit;
    r.power_limit = power_limit;
    r.stopping_dist = stop_dist;
    r.stopping_time = stop_time;
    zucSendCommand(s, r);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_drag_speed_limit_settings(pyCommandChannel* s, PyObject* o)
{
    ZUC_DRAG_SPEED_LIMIT drag_limit;
    double drag_tcp_speed_limit;
    if (!PyArg_ParseTuple(o, "d", &drag_tcp_speed_limit))
    {
        return NULL;
    }
    drag_limit.drag_tcp_speed_limit = drag_tcp_speed_limit;
    zucSendCommand(s, drag_limit);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_tool_offset(pyCommandChannel* s, PyObject* o)
{
    ZUC_TRAJ_SET_ROBOT_TOOL_OFFSET m;
    if (!PyArg_ParseTuple(o,
                          "dddddd",
                          &m.robotToolOffset.tran.x,
                          &m.robotToolOffset.tran.y,
                          &m.robotToolOffset.tran.z,
                          &m.robotToolOffset.a,
                          &m.robotToolOffset.b,
                          &m.robotToolOffset.c))
        return NULL;

    m.robotToolOffset.u = 0.0;
    m.robotToolOffset.v = 0.0;
    m.robotToolOffset.w = 0.0;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_tool_id(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_TOOL_ID m;
    if (!PyArg_ParseTuple(o, "ii", &m.robot_id, &m.id))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_joint_safety_settings(pyCommandChannel* s, PyObject* o)
{
    ZUC_JOINT_SAFETY jointSafetySettings;
    int robot_id;
    int jointNum, paramType;
    double jointVelLimit, jointErrLimit;

    if (!PyArg_ParseTuple(o, "iiidd", &robot_id, &jointNum, &paramType, &jointVelLimit, &jointErrLimit))
    {
        PyErr_Format(PyExc_TypeError, "set_joint_safety_settings(...) takes at least 5 arguments (%lu given)", (unsigned long)PyTuple_Size(o));
        return NULL;
    }
    if (PyTuple_Size(o) < 4)
    {
        PyErr_Format(PyExc_TypeError, "set_joint_safety_settings(...) takes at least 5 arguments (%lu given)", (unsigned long)PyTuple_Size(o));
        return NULL;
    }

    jointSafetySettings.robot_id = robot_id;
    jointSafetySettings.jointNum = jointNum;
    jointSafetySettings.paramType = paramType;
    jointSafetySettings.jointVelLimit = jointVelLimit;
    jointSafetySettings.jointErrLimit = jointErrLimit;
    jointSafetySettings.type = 0;

    zucSendCommand(s, jointSafetySettings);

    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* get_joint_safety_settings(pyCommandChannel* s, PyObject* o)
{
    ZUC_JOINT_SAFETY jointSafetySettings;
    jointSafetySettings.type = 1;
    zucSendCommand(s, jointSafetySettings);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* reset_step_status(pyCommandChannel* s, PyObject* o)
{
    ZUC_STEP_STATUS stepStatus;
    stepStatus.isSteping = 0;
    stepStatus.programRunTrigger = 0;
    stepStatus.stepEnd = 0;
    zucSendCommand(s, stepStatus);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_collision_option(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_COLLISION_OPTION setClsnOpt;
    if (!PyArg_ParseTuple(
            o, "iiid", &setClsnOpt.robot_id, &setClsnOpt.clsnDetectMethod, &setClsnOpt.clsnReactTypeWhenProgRun, &setClsnOpt.clsnReactionReboudnAng))
    {
        return NULL;
    }
    setClsnOpt.clsnReactionReboudnLen = setClsnOpt.clsnReactionReboudnAng * 10;  // @FIXME 应当在APP设置接口
    setClsnOpt.clsnReactionReboudnJAcc = 1440;
    setClsnOpt.clsnReactionReboudnJVel = 360;
    setClsnOpt.clsnReactionReboudnLAcc = 4000;
    setClsnOpt.clsnReactionReboudnLVel = 2000;
    zucSendCommand(s, setClsnOpt);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* setup_extio(pyCommandChannel* s, PyObject* o)
{
    int count = 0;
    PyObject* pyExtIOSetups;
    printf(" [setup_extio] :  \n");

    /*predefined arguments format*/
    // (count, [(type, name, (comm), {"di":disetup, "do":dosetup, "ai":aisetup, "ao":aosetup}), ...])
    if (!PyArg_ParseTuple(o, "iO", &count, &pyExtIOSetups))
    {
        printf(" [setup_extio] : input param error -----------1 \n");
        return NULL;
    }
    ZUC_EXTIO_SETUP m;
    m.extIOModCnt = count >= 0 ? count : 0;
    if (count > 0)
    {
        for (int i = 0; i < count; ++i)
        {
            PyObject* PuExtIOSetup = NULL;
            PuExtIOSetup = PyList_GetItem(pyExtIOSetups, i);

            char *modname, *ExtIOModInfo;
            Py_ssize_t namelen;
            int type;
            PyObject *commObj, *pinMap;
            if (!PyArg_ParseTuple(PuExtIOSetup, "is#OOs", &type, &modname, &namelen, &commObj, &pinMap, &ExtIOModInfo))
            {
                printf(" [setup_extio] : input param error -----------2 %d \n", i);
                return NULL;
            }

            m.extIOSetup[i].extType = type;
            namelen = namelen < MAX_EXTIO_NAME_LEN ? namelen : MAX_EXTIO_NAME_LEN;
            strncpy(m.extIOSetup[i].ExtIOModInfo, ExtIOModInfo, MAX_EXTIO_NAME_LEN);
            strncpy(m.extIOSetup[i].modName, modname, MAX_EXTIO_NAME_LEN);
            m.extIOSetup[i].modName[MAX_EXTIO_NAME_LEN - 1] = '\0';
            m.extIOSetup[i].ExtIOModInfo[MAX_EXTIO_NAME_LEN - 1] = '\0';

            if (type == 0)
            {  // MOD-RTU
                int baudrate, slaveid, datalength, parity, stopbits;
                if (!PyArg_ParseTuple(commObj, "iiiii", &baudrate, &datalength, &slaveid, &parity, &stopbits))
                {
                    printf(" [setup_extio] : input param error -----------3  %d \n", i);
                    return NULL;
                }
                m.extIOSetup[i].commInfo.rtuComm.baudrate = baudrate;
                m.extIOSetup[i].commInfo.rtuComm.databits = datalength;
                m.extIOSetup[i].commInfo.rtuComm.slaveId = slaveid;
                m.extIOSetup[i].commInfo.rtuComm.parity = parity;
                m.extIOSetup[i].commInfo.rtuComm.stopbits = stopbits;
                if (slaveid > 247 || slaveid < 0)
                {
                    printf(" [setup_extio] : input param error -----------slaveId != [0,247]   %d \n", i);
                    return NULL;
                }
            }
            else
            {  // MOD-TCP
                Py_ssize_t addrLen;
                int port, tcpSlaveId;
                char* ipaddr;
                if (!PyArg_ParseTuple(commObj, "s#ii", &ipaddr, &addrLen, &port, &tcpSlaveId))
                {
                    printf(" [setup_extio] : input param error -----------4  %d \n", i);
                    return NULL;
                }
                strcpy(m.extIOSetup[i].commInfo.tcpComm.ipaddr, ipaddr);
                m.extIOSetup[i].commInfo.tcpComm.port = port;
                m.extIOSetup[i].commInfo.tcpComm.slaveId = tcpSlaveId;
            }

            PyObject* pyDIMap = PyList_GetItem(pinMap, 0);
            if (!pyDIMap || !PyArg_ParseTuple(pyDIMap, "ii", &m.extIOSetup[i].diSetup.startRegAddr, &m.extIOSetup[i].diSetup.pinNum))
            {
                printf(" [setup_extio] : input param error -----------5  %d \n", i);
                return NULL;
            }
            PyObject* pyDOMap = PyList_GetItem(pinMap, 1);
            if (!pyDOMap || !PyArg_ParseTuple(pyDOMap, "ii", &m.extIOSetup[i].doSetup.startRegAddr, &m.extIOSetup[i].doSetup.pinNum))
            {
                printf(" [setup_extio] : input param error -----------6  %d \n", i);
                return NULL;
            }
            PyObject* pyAIMap = PyList_GetItem(pinMap, 2);
            if (!pyAIMap || !PyArg_ParseTuple(pyAIMap, "ii", &m.extIOSetup[i].aiSetup.startRegAddr, &m.extIOSetup[i].aiSetup.pinNum))
            {
                printf(" [setup_extio] : input param error -----------7  %d \n", i);
                return NULL;
            }
            PyObject* pyAOMap = PyList_GetItem(pinMap, 3);
            if (!pyAOMap || !PyArg_ParseTuple(pyAOMap, "ii", &m.extIOSetup[i].aoSetup.startRegAddr, &m.extIOSetup[i].aoSetup.pinNum))
            {
                printf(" [setup_extio] : input param error -----------8  %d \n", i);
                return NULL;
            }
        }
    }

    printf(" [setup_extio] :  zucSendCommand   \n");

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_extio_mode(pyCommandChannel* s, PyObject* o)
{
    ZUC_EXTIO_SETMODE m;
    if (!PyArg_ParseTuple(o, "i", &m.mode))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

//升级时切换至维护模式，屏蔽部分checksum功能
//后续可根据次状态屏蔽其余功能
static PyObject* set_maintenance_mode(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_MAINTENANCE_MODE m;
    if (!PyArg_ParseTuple(o, "i", &m.maintenance))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_torqsensor_comm(pyCommandChannel* s, PyObject* o)
{
    int commType;
    PyObject* commObj = NULL;
    ZUC_TORQSENSOR_SETCOMM m;

    if (!PyArg_ParseTuple(o, "i|O", &commType, &commObj))
        return NULL;

    if (commType == 0 && PyTuple_Size(o) == 2)
    {
        Py_ssize_t addrLen;
        int port;
        char* ipaddr;
        if (!PyArg_ParseTuple(commObj, "s#i", &ipaddr, &addrLen, &port))
            return NULL;
        strcpy(m.commCfg.tcpComm.ipaddr, ipaddr);
        m.commCfg.tcpComm.port = port;
    }
    else if (commType == 1)
    {
        // int baudrate, dataBit, stopBit, parity;
        // if(!PyArg_ParseTuple(commObj, "iiii", &baudrate, &dataBit, &stopBit, &parity)) {
        //     return NULL;
        // }
        // m.commCfg.rtuComm.baudrate = baudrate;
        // m.commCfg.rtuComm.databits = dataBit;
        // m.commCfg.rtuComm.stopbits = stopBit;
        // m.commCfg.rtuComm.parity = parity;
    }
    else
    {
        printf("incorrect argument number\n");
        return Py_None;
    }

    m.commType = commType;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_torqsensor_payload(pyCommandChannel* s, PyObject* o)
{
    ZUC_TORQSENSOR_SETPAYLOAD m;
    if (!PyArg_ParseTuple(o, "d(ddd)", &m.payload.m, &m.payload.p[0], &m.payload.p[1], &m.payload.p[2]))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_tool_payload(pyCommandChannel* s, PyObject* o)
{
    ZUC_TOOL_SETPAYLOAD m;
    if (!PyArg_ParseTuple(o, "d(ddd)", &m.payload.m, &m.payload.p[0], &m.payload.p[1], &m.payload.p[2]))
        return NULL;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_torqsensor_mode(pyCommandChannel* s, PyObject* o)
{
    ZUC_TORQSENSOR_SETMODE m;
    if (!PyArg_ParseTuple(o, "i", &m.mode))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_torqsensor_brand(pyCommandChannel* s, PyObject* o)
{
    ZUC_TORQSENSOR_SETBRAND m;
    if (!PyArg_ParseTuple(o, "i", &m.torqsensorBrand))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* disable_force_control(pyCommandChannel* s, PyObject* o)
{
    ZUC_TRAJ_DISABLE_FORCE_CONTROL m;
    if (!PyArg_ParseTuple(o, "i", &m.disableForceCtrl))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_compliant_type_enable(pyCommandChannel* s, PyObject* o)
{
    ZUC_TRAJ_SET_COMPLIANCE_ENABLE m;
    if (!PyArg_ParseTuple(o, "ii", &m.compliantType, &m.compliantEnable))
        return NULL;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_percentage_mode_level_value(pyCommandChannel* s, PyObject* o)
{
    ZUC_MOTION_PERCENTAGE_MODE_LEVEL_VALUE m;

    if (!PyArg_ParseTuple(o, "dd", &m.percentage_mode_level1_val, &m.percentage_mode_level2_val))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_free_button_opt(pyCommandChannel* s, PyObject* o)
{
    ZUC_MOTION_SET_FREE_BUTTON_OPT m;

    if (!PyArg_ParseTuple(o, "i", &m.freeButton))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_free_prog_button_opt(pyCommandChannel* s, PyObject* o)
{
    ZUC_MOTION_SET_FREE_PROG_BUTTON_OPT m;

    if (!PyArg_ParseTuple(o, "ii", &m.freeButton, &m.progButton))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_point_button_opt(pyCommandChannel* s, PyObject* o)
{
    ZUC_MOTION_SET_POINT_BUTTON_OPT m;

    if (!PyArg_ParseTuple(o, "i", &m.pointButton))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_sim_enable(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_SIM_ENABLE m;
    int value;
    if (!PyArg_ParseTuple(o, "i", &m.simEnable))
        return NULL;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_sim_robot(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_SIM_ROBOT m;
    int value;
    if (!PyArg_ParseTuple(o, "ii", &m.robot_id, &m.robotModel))
        return NULL;
    // printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>new robot model: %d,  %d,  %d,  %d \n",&m.robotModel.SN_old,m.robotModel.serial_num_0,m.robotModel.serial_num_1,m.robotModel.serial_num_2);
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* sync_zucsettings(pyCommandChannel* s, PyObject* o)
{
    ZUC_SYNC_ZUCSETTINGS m;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_percentage_mode(pyCommandChannel* s, PyObject* o)
{
    ZUC_MOTION_PERCENTAGE_MODE m;

    if (!PyArg_ParseTuple(o, "i", &m.percentage_level))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* setup_io_channel(pyCommandChannel* s, PyObject* o)
{
    ZUC_MOTION_SETUP_IO_CHANNEL m;
    if (!PyArg_ParseTuple(o, "iii", &m.channel_type, &m.channel_index, &m.channel_setting))
        return NULL;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_br_start_vol(pyCommandChannel* s, PyObject* o)
{
    ZUC_MOTION_SET_BR_STRT_VOLTAGE m;
    if (!PyArg_ParseTuple(o, "i", &m.starting_voltage))
        return NULL;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* shut_down(pyCommandChannel* s, PyObject* o)
{
    ZUC_SHUT_DOWN m;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* tio_add_rs_signal(pyCommandChannel* s, PyObject* o)
{
    ZUC_MBTIO_ADD_SIGNAL m;
    char* sigName;
    Py_ssize_t len;
    if (!PyArg_ParseTuple(o, "s#iii", &sigName, &len, &m.chnId, &m.type, &m.addr) || len > 128)
        return NULL;

    if (len == 128)
    {
        strncpy(m.sigName, sigName, len - 1);
    }
    else
    {
        strcpy(m.sigName, sigName);
        m.sigName[len] = '\0';
    }
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* tio_del_rs_signal(pyCommandChannel* s, PyObject* o)
{
    ZUC_MBTIO_DEL_SIGNAL m;
    char* sigName;
    Py_ssize_t len;
    if (!PyArg_ParseTuple(o, "s#", &sigName, &len) || len > 128)
        return NULL;

    if (len == 128)
    {
        strncpy(m.sigName, sigName, len - 1);
    }
    else
    {
        strcpy(m.sigName, sigName);
        m.sigName[len] = '\0';
    }
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* tio_update_rs_signal(pyCommandChannel* s, PyObject* o)
{
    ZUC_MBTIO_UPDATE_SIGNAL m;
    char* sigName;
    Py_ssize_t len;
    float freq;
    if (!PyArg_ParseTuple(o, "s#f", &sigName, &len, &freq) || len > 128)
        return NULL;

    if (len == 128)
    {
        strncpy(m.sigName, sigName, len - 1);
    }
    else
    {
        strcpy(m.sigName, sigName);
        m.sigName[len] = '\0';
    }
    m.freq = freq;
    zucSendCommand(s, m);
    esleep(0.1);

    PyObject* res = PyTuple_New(1);
    ZUC_STAT* stat = (ZUC_STAT*)s->s->get_address();
    PyTuple_SetItem(res, 0, PyLong_FromLong(stat->monitor.commState));
    return res;
}

static PyObject* tio_send_rs_command(pyCommandChannel* s, PyObject* o)
{
    ZUC_MBTIO_SEND_COMMAND m;

    PyObject* pyCmdList;
    if (!PyArg_ParseTuple(o, "iO", &m.chnId, &pyCmdList) || PyList_Size(pyCmdList) > 128)
    {
        return NULL;
    }

    for (int i = 0; i < PyList_Size(pyCmdList); i++)
    {
        PyObject* pyData = PyList_GetItem(pyCmdList, i);
        if (PyLong_Check(pyData))
        {
            long data = PyLong_AsLong(pyData);
            m.command[i] = data & 0xFF;
            m.length = i + 1;
        }
        else
        {
            PyErr_Format(PyExc_ValueError, "command should be an array of bytes");
            return NULL;
        }
    }

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* tio_set_pin_mode(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_SETUP_TIO_PIN_MODE cmd;
    if (!PyArg_ParseTuple(o, "ii", &cmd.type, &cmd.mode))
        return NULL;

    zucSendCommand(s, cmd);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* tio_set_rs485_chn_mode(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_SETUP_TIO_RS485_MODE cmd;
    if (!PyArg_ParseTuple(o, "ii", &cmd.id, &cmd.mode))
        return NULL;

    zucSendCommand(s, cmd);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* tio_set_rs485_chn_comm(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_SETUP_TIO_RS485_COMM cmd;
    if (!PyArg_ParseTuple(
            o, "i(iiiii)", &cmd.id, &cmd.rtuComm.slaveId, &cmd.rtuComm.baudrate, &cmd.rtuComm.databits, &cmd.rtuComm.stopbits, &cmd.rtuComm.parity))
        return NULL;

    zucSendCommand(s, cmd);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* tio_set_vout_ena(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_SETUP_TIO_VOUT_ENA cmd;
    if (!PyArg_ParseTuple(o, "i", &cmd.enable))
        return NULL;

    zucSendCommand(s, cmd);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* tio_set_vout_vol(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_SETUP_TIO_VOUT_VOL cmd;
    if (!PyArg_ParseTuple(o, "i", &cmd.voltage))
        return NULL;

    zucSendCommand(s, cmd);
    Py_INCREF(Py_None);
    return Py_None;
}

// 该接口未经GRPC调用
static PyObject* calib_dhparam(pyCommandChannel* s, PyObject* o)
{
    ZUC_TRAJ_CALIB_DHPARAM m;
    ZUC_STAT* stat = (ZUC_STAT*)s->s->get_address();

    if (stat->motion.robot_joint_num == 6)
    {
        if (!PyArg_ParseTuple(o,
                              "(ffffff)(ffffff)(ffffff)(ffffff)",
                              &m.dhParamError.alpha[0],
                              &m.dhParamError.alpha[1],
                              &m.dhParamError.alpha[2],
                              &m.dhParamError.alpha[3],
                              &m.dhParamError.alpha[4],
                              &m.dhParamError.alpha[5],
                              &m.dhParamError.a[0],
                              &m.dhParamError.a[1],
                              &m.dhParamError.a[2],
                              &m.dhParamError.a[3],
                              &m.dhParamError.a[4],
                              &m.dhParamError.a[5],
                              &m.dhParamError.d[0],
                              &m.dhParamError.d[1],
                              &m.dhParamError.d[2],
                              &m.dhParamError.d[3],
                              &m.dhParamError.d[4],
                              &m.dhParamError.d[5],
                              &m.dhParamError.joint_homeoff[0],
                              &m.dhParamError.joint_homeoff[1],
                              &m.dhParamError.joint_homeoff[2],
                              &m.dhParamError.joint_homeoff[3],
                              &m.dhParamError.joint_homeoff[4],
                              &m.dhParamError.joint_homeoff[5]))
            return NULL;
    }
    else if (stat->motion.robot_joint_num == 7)
    {
        if (!PyArg_ParseTuple(o,
                              "(fffffff)(fffffff)(fffffff)(fffffff)",
                              &m.dhParamError.alpha[0],
                              &m.dhParamError.alpha[1],
                              &m.dhParamError.alpha[2],
                              &m.dhParamError.alpha[3],
                              &m.dhParamError.alpha[4],
                              &m.dhParamError.alpha[5],
                              &m.dhParamError.alpha[6],
                              &m.dhParamError.a[0],
                              &m.dhParamError.a[1],
                              &m.dhParamError.a[2],
                              &m.dhParamError.a[3],
                              &m.dhParamError.a[4],
                              &m.dhParamError.a[5],
                              &m.dhParamError.a[6],
                              &m.dhParamError.d[0],
                              &m.dhParamError.d[1],
                              &m.dhParamError.d[2],
                              &m.dhParamError.d[3],
                              &m.dhParamError.d[4],
                              &m.dhParamError.d[5],
                              &m.dhParamError.d[6],
                              &m.dhParamError.joint_homeoff[0],
                              &m.dhParamError.joint_homeoff[1],
                              &m.dhParamError.joint_homeoff[2],
                              &m.dhParamError.joint_homeoff[3],
                              &m.dhParamError.joint_homeoff[4],
                              &m.dhParamError.joint_homeoff[5],
                              &m.dhParamError.joint_homeoff[6]))
            return NULL;
    }
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_funcdi(pyCommandChannel* s, PyObject* o)
{
    ZUC_MOTION_SET_FUNCDI m;

    if (!PyArg_ParseTuple(o,
                          "((hh)(hh)(hh)(hh)(hh)(hh)(hh)(hh)(hh)(hh)(hh)(hh)(hh)(hh)(hh))",
                          &m.funcIOMap.task_run_di.type,
                          &m.funcIOMap.task_run_di.index,
                          &m.funcIOMap.task_pause_di.type,
                          &m.funcIOMap.task_pause_di.index,
                          &m.funcIOMap.task_resume_di.type,
                          &m.funcIOMap.task_resume_di.index,
                          &m.funcIOMap.task_stop_di.type,
                          &m.funcIOMap.task_stop_di.index,
                          &m.funcIOMap.power_on_di.type,
                          &m.funcIOMap.power_on_di.index,
                          &m.funcIOMap.power_off_di.type,
                          &m.funcIOMap.power_off_di.index,
                          &m.funcIOMap.servo_enable_di.type,
                          &m.funcIOMap.servo_enable_di.index,
                          &m.funcIOMap.servo_disable_di.type,
                          &m.funcIOMap.servo_disable_di.index,
                          &m.funcIOMap.percentage_mode_level1_di.type,
                          &m.funcIOMap.percentage_mode_level1_di.index,
                          &m.funcIOMap.stop_mode_di.type,
                          &m.funcIOMap.stop_mode_di.index,
                          &m.funcIOMap.back_to_inipos_di.type,
                          &m.funcIOMap.back_to_inipos_di.index,
                          &m.funcIOMap.percentage_mode_level2_di.type,
                          &m.funcIOMap.percentage_mode_level2_di.index,
                          &m.funcIOMap.clear_error_di.type,
                          &m.funcIOMap.clear_error_di.index,
                          &m.funcIOMap.drag_mode_di.type,
                          &m.funcIOMap.drag_mode_di.index,
                          &m.funcIOMap.drag_mode_disable_di.type,
                          &m.funcIOMap.drag_mode_disable_di.index))  //
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_funcdo(pyCommandChannel* s, PyObject* o)  //
{
    ZUC_MOTION_SET_FUNCDO m;
    //FIXME_lin
    if (!PyArg_ParseTuple(
            o,
            "(((hh)(hh)(hh)(hh))((hh)(hh)(hh)(hh))((hh)(hh)(hh)(hh))((hh)(hh)(hh)(hh))((hh)(hh)(hh)(hh))((hh)(hh)(hh)(hh))((hh)(hh)(hh)(hh))((hh)(hh)(hh)(hh))("
            "(hh)(hh)(hh)(hh))((hh)(hh)(hh)(hh))((hh)(hh)(hh)(hh))((hh)(hh)(hh)(hh))((hh)(hh)(hh)(hh))((hh)(hh)(hh)(hh))((hh)(hh)(hh)(hh))((hh)(hh)(hh)(hh))(("
            "hh)(hh)(hh)(hh))((hh)(hh)(hh)(hh)))",

            &m.funcIOMap.task_idle_do[0].type,
            &m.funcIOMap.task_idle_do[0].index,
            &m.funcIOMap.task_idle_do[1].type,
            &m.funcIOMap.task_idle_do[1].index,
            &m.funcIOMap.task_idle_do[2].type,
            &m.funcIOMap.task_idle_do[2].index,
            &m.funcIOMap.task_idle_do[3].type,
            &m.funcIOMap.task_idle_do[3].index,
            &m.funcIOMap.task_paused_do[0].type,
            &m.funcIOMap.task_paused_do[0].index,
            &m.funcIOMap.task_paused_do[1].type,
            &m.funcIOMap.task_paused_do[1].index,
            &m.funcIOMap.task_paused_do[2].type,
            &m.funcIOMap.task_paused_do[2].index,
            &m.funcIOMap.task_paused_do[3].type,
            &m.funcIOMap.task_paused_do[3].index,
            &m.funcIOMap.task_running_do[0].type,
            &m.funcIOMap.task_running_do[0].index,
            &m.funcIOMap.task_running_do[1].type,
            &m.funcIOMap.task_running_do[1].index,
            &m.funcIOMap.task_running_do[2].type,
            &m.funcIOMap.task_running_do[2].index,
            &m.funcIOMap.task_running_do[3].type,
            &m.funcIOMap.task_running_do[3].index,
            &m.funcIOMap.stat_fault_do[0].type,
            &m.funcIOMap.stat_fault_do[0].index,
            &m.funcIOMap.stat_fault_do[1].type,
            &m.funcIOMap.stat_fault_do[1].index,
            &m.funcIOMap.stat_fault_do[2].type,
            &m.funcIOMap.stat_fault_do[2].index,
            &m.funcIOMap.stat_fault_do[3].type,
            &m.funcIOMap.stat_fault_do[3].index,
            &m.funcIOMap.stat_poweron_do[0].type,
            &m.funcIOMap.stat_poweron_do[0].index,
            &m.funcIOMap.stat_poweron_do[1].type,
            &m.funcIOMap.stat_poweron_do[1].index,
            &m.funcIOMap.stat_poweron_do[2].type,
            &m.funcIOMap.stat_poweron_do[2].index,
            &m.funcIOMap.stat_poweron_do[3].type,
            &m.funcIOMap.stat_poweron_do[3].index,
            &m.funcIOMap.stat_enabled_do[0].type,
            &m.funcIOMap.stat_enabled_do[0].index,
            &m.funcIOMap.stat_enabled_do[1].type,
            &m.funcIOMap.stat_enabled_do[1].index,
            &m.funcIOMap.stat_enabled_do[2].type,
            &m.funcIOMap.stat_enabled_do[2].index,
            &m.funcIOMap.stat_enabled_do[3].type,
            &m.funcIOMap.stat_enabled_do[3].index,
            &m.funcIOMap.stat_motion_do[0].type,
            &m.funcIOMap.stat_motion_do[0].index,
            &m.funcIOMap.stat_motion_do[1].type,
            &m.funcIOMap.stat_motion_do[1].index,
            &m.funcIOMap.stat_motion_do[2].type,
            &m.funcIOMap.stat_motion_do[2].index,
            &m.funcIOMap.stat_motion_do[3].type,
            &m.funcIOMap.stat_motion_do[3].index,
            &m.funcIOMap.stat_static_do[0].type,
            &m.funcIOMap.stat_static_do[0].index,
            &m.funcIOMap.stat_static_do[1].type,
            &m.funcIOMap.stat_static_do[1].index,
            &m.funcIOMap.stat_static_do[2].type,
            &m.funcIOMap.stat_static_do[2].index,
            &m.funcIOMap.stat_static_do[3].type,
            &m.funcIOMap.stat_static_do[3].index,
            &m.funcIOMap.stat_boot_do[0].type,
            &m.funcIOMap.stat_boot_do[0].index,
            &m.funcIOMap.stat_boot_do[1].type,
            &m.funcIOMap.stat_boot_do[1].index,
            &m.funcIOMap.stat_boot_do[2].type,
            &m.funcIOMap.stat_boot_do[2].index,
            &m.funcIOMap.stat_boot_do[3].type,
            &m.funcIOMap.stat_boot_do[3].index,
            &m.funcIOMap.stat_emergency_stop_do[0].type,
            &m.funcIOMap.stat_emergency_stop_do[0].index,
            &m.funcIOMap.stat_emergency_stop_do[1].type,
            &m.funcIOMap.stat_emergency_stop_do[1].index,
            &m.funcIOMap.stat_emergency_stop_do[2].type,
            &m.funcIOMap.stat_emergency_stop_do[2].index,
            &m.funcIOMap.stat_emergency_stop_do[3].type,
            &m.funcIOMap.stat_emergency_stop_do[3].index,
            &m.funcIOMap.stat_percentage_mode_level2_do[0].type,
            &m.funcIOMap.stat_percentage_mode_level2_do[0].index,
            &m.funcIOMap.stat_percentage_mode_level2_do[1].type,
            &m.funcIOMap.stat_percentage_mode_level2_do[1].index,
            &m.funcIOMap.stat_percentage_mode_level2_do[2].type,
            &m.funcIOMap.stat_percentage_mode_level2_do[2].index,
            &m.funcIOMap.stat_percentage_mode_level2_do[3].type,
            &m.funcIOMap.stat_percentage_mode_level2_do[3].index,
            &m.funcIOMap.stat_sys_protective_stop_do[0].type,
            &m.funcIOMap.stat_sys_protective_stop_do[0].index,
            &m.funcIOMap.stat_sys_protective_stop_do[1].type,
            &m.funcIOMap.stat_sys_protective_stop_do[1].index,
            &m.funcIOMap.stat_sys_protective_stop_do[2].type,
            &m.funcIOMap.stat_sys_protective_stop_do[2].index,
            &m.funcIOMap.stat_sys_protective_stop_do[3].type,
            &m.funcIOMap.stat_sys_protective_stop_do[3].index,
            &m.funcIOMap.stat_initial_pose_do[0].type,
            &m.funcIOMap.stat_initial_pose_do[0].index,
            &m.funcIOMap.stat_initial_pose_do[1].type,
            &m.funcIOMap.stat_initial_pose_do[1].index,
            &m.funcIOMap.stat_initial_pose_do[2].type,
            &m.funcIOMap.stat_initial_pose_do[2].index,
            &m.funcIOMap.stat_initial_pose_do[3].type,
            &m.funcIOMap.stat_initial_pose_do[3].index,
            &m.funcIOMap.stat_drag_mode_do[0].type,
            &m.funcIOMap.stat_drag_mode_do[0].index,
            &m.funcIOMap.stat_drag_mode_do[1].type,
            &m.funcIOMap.stat_drag_mode_do[1].index,
            &m.funcIOMap.stat_drag_mode_do[2].type,
            &m.funcIOMap.stat_drag_mode_do[2].index,
            &m.funcIOMap.stat_drag_mode_do[3].type,
            &m.funcIOMap.stat_drag_mode_do[3].index,
            &m.funcIOMap.stat_collision_mode_do[0].type,
            &m.funcIOMap.stat_collision_mode_do[0].index,
            &m.funcIOMap.stat_collision_mode_do[1].type,
            &m.funcIOMap.stat_collision_mode_do[1].index,
            &m.funcIOMap.stat_collision_mode_do[2].type,
            &m.funcIOMap.stat_collision_mode_do[2].index,
            &m.funcIOMap.stat_collision_mode_do[3].type,
            &m.funcIOMap.stat_collision_mode_do[3].index,
            &m.funcIOMap.stat_percentage_mode_level1_do[0].type,
            &m.funcIOMap.stat_percentage_mode_level1_do[0].index,
            &m.funcIOMap.stat_percentage_mode_level1_do[1].type,
            &m.funcIOMap.stat_percentage_mode_level1_do[1].index,
            &m.funcIOMap.stat_percentage_mode_level1_do[2].type,
            &m.funcIOMap.stat_percentage_mode_level1_do[2].index,
            &m.funcIOMap.stat_percentage_mode_level1_do[3].type,
            &m.funcIOMap.stat_percentage_mode_level1_do[3].index,
            &m.funcIOMap.stat_manual_mode_do[0].type,
            &m.funcIOMap.stat_manual_mode_do[0].index,
            &m.funcIOMap.stat_manual_mode_do[1].type,
            &m.funcIOMap.stat_manual_mode_do[1].index,
            &m.funcIOMap.stat_manual_mode_do[2].type,
            &m.funcIOMap.stat_manual_mode_do[2].index,
            &m.funcIOMap.stat_manual_mode_do[3].type,
            &m.funcIOMap.stat_manual_mode_do[3].index,
            &m.funcIOMap.stat_auto_mode_do[0].type,
            &m.funcIOMap.stat_auto_mode_do[0].index,
            &m.funcIOMap.stat_auto_mode_do[1].type,
            &m.funcIOMap.stat_auto_mode_do[1].index,
            &m.funcIOMap.stat_auto_mode_do[2].type,
            &m.funcIOMap.stat_auto_mode_do[2].index,
            &m.funcIOMap.stat_auto_mode_do[3].type,
            &m.funcIOMap.stat_auto_mode_do[3].index))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_safety_funcdi(pyCommandChannel* s, PyObject* o)  //
{
    ZUC_MOTION_SET_SAFETY_FUNCDI m;
    /* app set ch1 ch2 ch3  ch4 ch5 ch6 ch7 ch8
     but controler needs  ch4 ch3 ch2 ch1  ch8 ch7 ch6 ch5*/
    if (!PyArg_ParseTuple(o,
                          "(hhhhhhhh)",
                          &m.sFuncDISettings.diChnFunc[3],
                          &m.sFuncDISettings.diChnFunc[2],
                          &m.sFuncDISettings.diChnFunc[1],
                          &m.sFuncDISettings.diChnFunc[0],
                          &m.sFuncDISettings.diChnFunc[7],
                          &m.sFuncDISettings.diChnFunc[6],
                          &m.sFuncDISettings.diChnFunc[5],
                          &m.sFuncDISettings.diChnFunc[4]))
        return NULL;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_safety_funcdo(pyCommandChannel* s, PyObject* o)
{
    ZUC_MOTION_SET_SAFETY_FUNCDO m;
    /* app set ch1 ch2 ch3  ch4 ch5 ch6 ch7 ch8
     but controler needs  ch4 ch3 ch2 ch1  ch8 ch7 ch6 ch5*/
    if (!PyArg_ParseTuple(o,
                          "(hhhhhhhh)",
                          &m.sFuncDOSettings.doChnFunc[3],
                          &m.sFuncDOSettings.doChnFunc[2],
                          &m.sFuncDOSettings.doChnFunc[1],
                          &m.sFuncDOSettings.doChnFunc[0],
                          &m.sFuncDOSettings.doChnFunc[7],
                          &m.sFuncDOSettings.doChnFunc[6],
                          &m.sFuncDOSettings.doChnFunc[5],
                          &m.sFuncDOSettings.doChnFunc[4]))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_auto_work(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_SET_AUTOWORK m;

    char* workfile;
    Py_ssize_t len;
    if (!PyArg_ParseTuple(o, "is#", &m.enable, &workfile, &len))
        return NULL;
    if (unsigned(len) > sizeof(m.filename) - 1)
    {
        PyErr_Format(PyExc_ValueError, "File name limited to %zu characters", sizeof(m.filename) - 1);
        return NULL;
    }
    strcpy(m.filename, workfile);

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_script_backup(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_SET_SCRIPT_BACKUP m;
    if (!PyArg_ParseTuple(o, "if", &m.enable, &m.interval))
        return NULL;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_robot_name(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_ROBOT_NAME m;

    char* robotName;
    Py_ssize_t len;
    if (!PyArg_ParseTuple(o, "s#", &robotName, &len))
        return NULL;
    if (unsigned(len) > sizeof(m.name) - 1)
    {
        PyErr_Format(PyExc_ValueError, "Robot name limited to %zu characters", sizeof(m.name) - 1);
        return NULL;
    }
    strcpy(m.name, robotName);

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_cab_id(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_CABINET_ID m;
    if (!PyArg_ParseTuple(o, "i", &m.cabinet_id))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_custom_pose(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_CUSTOM_POSE m;
    PyObject* valObj;

    if (!PyArg_ParseTuple(o, "iiO", &m.robot_id, &m.type, &valObj))
    {
        return NULL;
    }
    if (m.type <= 2)
    {
        if (!PyArg_ParseTuple(valObj, "ddddddd", &m.jpos[0], &m.jpos[1], &m.jpos[2], &m.jpos[3], &m.jpos[4], &m.jpos[5], &m.jpos[6]))
        {
            Py_INCREF(Py_None);
            return Py_None;
        }
    }
    else if (!PyArg_Parse(valObj, "d", &m.poseTol) || m.poseTol < 0.001)
    {
        printf("invalid pose tollerance\n");
        Py_INCREF(Py_None);
        return Py_None;
    }

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_collision_gm_record_enable(pyCommandChannel* s, PyObject* o)
{
    ZUC_COLLISION_GM_RECORD_ENABLE m;
    if (!PyArg_ParseTuple(o, "i", &m.gm_record_enable))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_identify_traj_startpos(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_SET_PAYLOAD_IDENTIFY_TRAJ_STARTPOS m;

    if (!PyArg_ParseTuple(o,
                          "i(fffffff)",
                          &m.trajId,
                          &m.jointTraj.startJntPos[0],
                          &m.jointTraj.startJntPos[1],
                          &m.jointTraj.startJntPos[2],
                          &m.jointTraj.startJntPos[3],
                          &m.jointTraj.startJntPos[4],
                          &m.jointTraj.startJntPos[5],
                          &m.jointTraj.startJntPos[6]))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_identify_traj_endpos(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_SET_PAYLOAD_IDENTIFY_TRAJ_ENDPOS m;

    if (!PyArg_ParseTuple(o,
                          "i(fffffff)",
                          &m.trajId,
                          &m.jointTraj.endJntPos[0],
                          &m.jointTraj.endJntPos[1],
                          &m.jointTraj.endJntPos[2],
                          &m.jointTraj.endJntPos[3],
                          &m.jointTraj.endJntPos[4],
                          &m.jointTraj.endJntPos[5],
                          &m.jointTraj.endJntPos[6]))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* start_identify_traj(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_START_PAYLOAD_IDENTIFY_TRAJ m;
    if (!PyArg_ParseTuple(o, "ii", &m.trajId, &m.withPayload))
        return NULL;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* get_identify_result(pyCommandChannel* s, PyObject* o)
{
    if (PyTuple_Size(o) != 0)
    {
        PyErr_Format(PyExc_TypeError, "get_identify_result(...) takes no arguments (%lu given)", (unsigned long)PyTuple_Size(o));
        return NULL;
    }

    ZUC_GET_PAYLOAD_IDENTIFY_RESULT m;
    zucSendCommand(s, m);
    esleep(0.1);

    ZUC_STAT* stat = (ZUC_STAT*)s->s->get_address();

    PyObject* res = PyTuple_New(3);
    if (stat->usrOpeErrCode != 0)
    {
        PyTuple_SetItem(res, 0, PyFloat_FromDouble(0.0));
        PyObject* com = PyTuple_New(3);
        for (int i = 0; i < 3; i++) { PyTuple_SetItem(com, i, PyFloat_FromDouble(0.0)); }
        PyTuple_SetItem(res, 1, com);
        PyTuple_SetItem(res, 2, PyLong_FromLong(-1));
    }
    else
    {
        PyTuple_SetItem(res, 0, PyFloat_FromDouble(stat->motion.utilPayload.m));
        PyObject* com = PyTuple_New(3);
        for (int i = 0; i < 3; i++) { PyTuple_SetItem(com, i, PyFloat_FromDouble(stat->motion.utilPayload.p[i])); }
        PyTuple_SetItem(res, 1, com);
        PyTuple_SetItem(res, 2, PyLong_FromLong(0));
    }

    return res;
}

static PyObject* get_payload_auto_identify_result(pyCommandChannel* s, PyObject* o)
{
    PyObject* res = PyTuple_New(2);
    double PayLoad[4] = {0};
    GetPayLoad(PayLoad);
    PyObject* com = PyTuple_New(3);
    for (int i = 0; i < 3; i++) { PyTuple_SetItem(com, i, PyFloat_FromDouble(PayLoad[i])); }
    PyTuple_SetItem(res, 0, PyFloat_FromDouble(PayLoad[3]));
    PyTuple_SetItem(res, 1, com);
    return res;
}

static PyObject* reset_identify_status(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_RESET_PAYLOAD_IDENTIFY_STATUS m;
    if (!PyArg_ParseTuple(o, "i", &m.userOpt))
        return NULL;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* stop_identify_friction(pyCommandChannel* s, PyObject* o)
{
    ZUC_STOP_IDENTIFY_FRICTION m;
    if (!PyArg_ParseTuple(o, "i", &m.friction_identify_joint_index))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* start_identify_friction(pyCommandChannel* s, PyObject* o)
{
    ZUC_START_IDENTIFY_FRICTION m;
    if (!PyArg_ParseTuple(o, "i", &m.friction_identify_joint_index))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_identify_friction_end_pos(pyCommandChannel* s, PyObject* o)
{
    ZUC_IDENTIFY_FRICTION_END_POS m;

    if (!PyArg_ParseTuple(o,
                          "iddddddd",
                          &m.friction_identify_joint_index,
                          &m.friction_identify_end_pos[0],
                          &m.friction_identify_end_pos[1],
                          &m.friction_identify_end_pos[2],
                          &m.friction_identify_end_pos[3],
                          &m.friction_identify_end_pos[4],
                          &m.friction_identify_end_pos[5],
                          &m.friction_identify_end_pos[6]))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_identify_friction_start_pos(pyCommandChannel* s, PyObject* o)
{
    ZUC_IDENTIFY_FRICTION_START_POS m;
    if (!PyArg_ParseTuple(o,
                          "iddddddd",
                          &m.friction_identify_joint_index,
                          &m.friction_identify_start_pos[0],
                          &m.friction_identify_start_pos[1],
                          &m.friction_identify_start_pos[2],
                          &m.friction_identify_start_pos[3],
                          &m.friction_identify_start_pos[4],
                          &m.friction_identify_start_pos[5],
                          &m.friction_identify_start_pos[6]))
        return NULL;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_servo_dynamics_para(pyCommandChannel* s, PyObject* o)
{
    ZUC_SERVO_DYNAMICS_PARA m;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_servo_friction(pyCommandChannel* s, PyObject* o)
{
    ZUC_SERVO_FRICTION m;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_backdrive_on(pyCommandChannel* s, PyObject* o)
{
    ZUC_BACKDRIVE_ON m;
    if (!PyArg_ParseTuple(o, "i", &m.enable))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* get_RS485_message(pyCommandChannel* s, PyObject* o)
{
    ZUC_RS485_MESSAGE m;
    if (!PyArg_ParseTuple(o, "ii", &m.chn, &m.len))
        return NULL;
    zucSendCommand(s, m);
    esleep(0.1);

    ZUC_STAT* stat = (ZUC_STAT*)s->s->get_address();

    PyObject* res = PyTuple_New(2);
    PyObject* msg;
    if (stat->tio[0].rs485_msg_len > 0)
    {
        msg = short_array(stat->tio[0].rs485_msg, stat->tio[0].rs485_msg_len);
    }
    else
    {
        // 没有数据就回两个0
        short msg_res[2] = {0, 0};
        msg = short_array(msg_res, 2);
    }
    PyTuple_SetItem(res, 0, PyLong_FromLong(stat->tio[0].rs485_msg_len));
    PyTuple_SetItem(res, 1, msg);
    return res;
}

static PyObject* set_TIO_RS485_sig_prog_stat(pyCommandChannel* s, PyObject* o)
{
    ZUC_TIO_RS485_SIG_PROG_STAT m;
    char* sigName;
    Py_ssize_t len;
    if (!PyArg_ParseTuple(o, "s#i", &sigName, &len, &m.prog_stat) || len > 128)
        return NULL;

    if (len == 128)
    {
        strncpy(m.sig_name, sigName, len - 1);
    }
    else
    {
        strcpy(m.sig_name, sigName);
        m.sig_name[len] = '\0';
    }

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_identify_cycle_time(pyCommandChannel* s, PyObject* o)
{
    ZUC_IDENTIFY_CYCLE_TIME m;
    if (!PyArg_ParseTuple(o, "i", &m.identify_cycle_time))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* start_identify_dynamics(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_START_DYNAMICS_IDENTIFY m;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* stop_identify_dynamics(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_STOP_DYNAMICS_IDENTIFY m;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* setup_mb_slave(pyCommandChannel* s, PyObject* o)
{
    ZUC_SETUP_MODBUS_SLAVE m;
    int commType;
    PyObject* commObj = NULL;

    if (!PyArg_ParseTuple(o, "iO", &commType, &commObj))
        return NULL;

    if (commType == 0)
    {
        int slaveId, baudrate, dataBit, stopBit, parity;
        if (!PyArg_ParseTuple(commObj, "iiiii", &slaveId, &baudrate, &dataBit, &stopBit, &parity))
        {
            return NULL;
        }
        m.mbComm.rtuComm.slaveId = slaveId;
        m.mbComm.rtuComm.baudrate = baudrate;
        m.mbComm.rtuComm.databits = dataBit;
        m.mbComm.rtuComm.stopbits = stopBit;
        m.mbComm.rtuComm.parity = parity;
    }
    else if (commType == 1)
    {
        Py_ssize_t addrLen;
        int port;
        char* ipaddr;
        if (!PyArg_ParseTuple(commObj, "s#i", &ipaddr, &addrLen, &port))
            return NULL;
        strcpy(m.mbComm.tcpComm.ipaddr, ipaddr);
        m.mbComm.tcpComm.port = port;
    }
    else
    {
        return Py_None;
    }

    m.commType = commType;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* mist(pyCommandChannel* s, PyObject* o) { return Py_None; }

static PyObject* flood(pyCommandChannel* s, PyObject* o) { return Py_None; }

static PyObject* brake(pyCommandChannel* s, PyObject* o) { return Py_None; }

static PyObject* load_tool_table(pyCommandChannel* s, PyObject* o) { return Py_None; }

static PyObject* zucabort(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_ABORT m;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

// jog(JOG_STOP,       jjogmode, ja_value)
// jog(JOG_CONTINUOUS, jjogmode, ja_value, speed)
// jog(JOG_INCREMENT,  jjogmode, ja_value, speed, increment)
static PyObject* jog(pyCommandChannel* s, PyObject* o)
{
    int robid = -1;
    int fn;        // 模式
    int ja_value;  // 轴
    int jjogmode;  // 笛卡尔1 or 关节0
    double vel, inc;

    if (!PyArg_ParseTuple(o, "iii|dd", &fn, &jjogmode, &ja_value, &vel, &inc))
    {
        return NULL;
    }

    if (fn == LOCAL_JOG_STOP)
    {
        if (PyTuple_Size(o) != 3)
        {
            PyErr_Format(PyExc_TypeError, "jog(JOG_STOP, ...) takes 3 arguments (%lu given)", (unsigned long)PyTuple_Size(o));
            return NULL;
        }
        ZUC_JOG_STOP abort;
        abort.joint_or_axis = ja_value;
        abort.jjogmode = jjogmode;
        zucSendCommand(s, abort);
        ZUC_MOTION_ABORT motion_abort;
        zucSendCommand(s, motion_abort);
    }
    else if (fn == LOCAL_JOG_CONTINUOUS)
    {
        if (PyTuple_Size(o) != 4)
        {
            PyErr_Format(PyExc_TypeError, "jog(JOG_CONTINUOUS, ...) takes 4 arguments (%lu given)", (unsigned long)PyTuple_Size(o));
            return NULL;
        }
        ZUC_JOG_CONT cont;
        cont.joint_or_axis = ja_value;
        cont.vel = vel;
        cont.jjogmode = jjogmode;
        zucSendCommand(s, cont);
    }
    else if (fn == LOCAL_JOG_INCREMENT)
    {
        if (PyTuple_Size(o) != 5)
        {
            PyErr_Format(PyExc_TypeError, "jog(JOG_INCREMENT, ...) takes 5 arguments (%lu given)", (unsigned long)PyTuple_Size(o));
            return NULL;
        }

        ZUC_JOG_INCR incr;
        incr.joint_or_axis = ja_value;
        incr.vel = vel;
        incr.incr = inc;
        incr.jjogmode = jjogmode;
        zucSendCommand(s, incr);
    }
    else if (fn == LOCAL_JOG_ABS)
    {
        if (PyTuple_Size(o) != 5)
        {
            PyErr_Format(PyExc_TypeError, "jog(JOG_ABS, ...) takes 5 arguments (%lu given)", (unsigned long)PyTuple_Size(o));
            return NULL;
        }

        ZUC_JOG_ABS jabs;
        jabs.joint_or_axis = ja_value;
        jabs.vel = vel;
        jabs.pos = inc;
        jabs.jjogmode = jjogmode;
        zucSendCommand(s, jabs);
    }
    else
    {
        PyErr_Format(PyExc_TypeError, "jog() first argument must be JOG_xxx");
        return NULL;
    }

    Py_INCREF(Py_None);
    return Py_None;
}
static PyObject* axis_power(pyCommandChannel* s, PyObject* o)
{
    ZUC_AXIS_POWER cmd;
    if (!PyArg_ParseTuple(o, "ii", &cmd.axisid, &cmd.enable))
    {
        PyErr_Format(PyExc_TypeError, "axis_power(axis,enable) takes at least 2 arguments (%lu given)", (unsigned long)PyTuple_Size(o));
        return NULL;
    }
    zucSendCommand(s, cmd);
    Py_INCREF(Py_None);
    return Py_None;
}
static PyObject* axis_reset(pyCommandChannel* s, PyObject* o)
{
    ZUC_AXIS_RESET cmd;
    if (!PyArg_ParseTuple(o, "i", &cmd.axisid))
    {
        PyErr_Format(PyExc_TypeError, "axis_reset(axis) takes at least 1 arguments (%lu given)", (unsigned long)PyTuple_Size(o));
        return NULL;
    }
    zucSendCommand(s, cmd);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* scb_power_on(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_SET_STATE m;
    m.state = ZUC_TASK_STATE_POWERED_ON;
    m.robid = -1;
    if (!PyArg_ParseTuple(o, "|i", &m.robid))
    {
        return NULL;  // 解析失败，返回NULL
    }

    printf("robid:%d\n", m.robid);
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* scb_power_off(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_SET_STATE m;
    m.state = ZUC_TASK_STATE_POWERED_OFF;
    m.robid = -1;
    PyArg_ParseTuple(o, "|i", &m.robid);

    printf("robid:%d\n", m.robid);
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* rob_enable(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_SET_STATE m;
    m.state = ZUC_TASK_STATE_ON;
    m.robid = -1;
    if (!PyArg_ParseTuple(o, "|i", &m.robid))
        return NULL;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* rob_disable(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_SET_STATE m;
    m.state = ZUC_TASK_STATE_OFF;
    m.robid = -1;
    if (!PyArg_ParseTuple(o, "|i", &m.robid))
        return NULL;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* robot_motion_abort(pyCommandChannel* s, PyObject* o)
{
    ZUC_MOTION_ABORT m;
    if (!PyArg_ParseTuple(o, "i", &m.robot_id))
    {
        return NULL;
    }
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* robot_jog(pyCommandChannel* s, PyObject* o)
{
    int robid = -1;
    int fn;        // 模式
    int ja_value;  // 轴
    int jjogmode;  // 关节 or 笛卡尔/W or 笛卡尔/T
    double vel, inc;

    if (!PyArg_ParseTuple(o, "iiii|dd", &robid, &fn, &jjogmode, &ja_value, &vel, &inc))
    {
        return NULL;
    }
    printf("robid:%d\n", robid);

    if (fn == LOCAL_JOG_STOP)
    {
        if (PyTuple_Size(o) != 4)
        {
            PyErr_Format(PyExc_TypeError, "jog(JOG_STOP, ...) takes 4 arguments (%lu given)", (unsigned long)PyTuple_Size(o));
            return NULL;
        }
        ZUC_JOG_STOP abort;
        abort.robot_id = robid;
        abort.joint_or_axis = ja_value;
        abort.jjogmode = jjogmode;
        zucSendCommand(s, abort);
    }
    else if (fn == LOCAL_JOG_CONTINUOUS)
    {
        if (PyTuple_Size(o) != 5)
        {
            PyErr_Format(PyExc_TypeError, "jog(JOG_CONTINUOUS, ...) takes 5 arguments (%lu given)", (unsigned long)PyTuple_Size(o));
            return NULL;
        }
        ZUC_JOG_CONT cont;
        cont.robot_id = robid;
        cont.joint_or_axis = ja_value;
        cont.vel = vel;
        cont.jjogmode = jjogmode;
        zucSendCommand(s, cont);
    }
    else if (fn == LOCAL_JOG_INCREMENT)
    {
        if (PyTuple_Size(o) != 6)
        {
            PyErr_Format(PyExc_TypeError, "jog(JOG_INCREMENT, ...) takes 6 arguments (%lu given)", (unsigned long)PyTuple_Size(o));
            return NULL;
        }

        ZUC_JOG_INCR incr;
        incr.robot_id = robid;
        incr.joint_or_axis = ja_value;
        incr.vel = vel;
        incr.incr = inc;
        incr.jjogmode = jjogmode;
        zucSendCommand(s, incr);
    }
    else if (fn == LOCAL_JOG_ABS)
    {
        if (PyTuple_Size(o) != 6)
        {
            PyErr_Format(PyExc_TypeError, "jog(JOG_ABS, ...) takes 6 arguments (%lu given)", (unsigned long)PyTuple_Size(o));
            return NULL;
        }
        ZUC_JOG_ABS jabs;
        jabs.robot_id = robid;
        jabs.joint_or_axis = ja_value;
        jabs.vel = vel;
        jabs.pos = inc;
        jabs.jjogmode = jjogmode;
        zucSendCommand(s, jabs);
    }
    else
    {
        PyErr_Format(PyExc_TypeError, "jog() first argument must be JOG_xxx");
        return NULL;
    }

    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* robot_set_drag_enable(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_DRAG_ENABLE m;
    if (!PyArg_ParseTuple(o, "ii", &m.robot_id, &m.enable))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

// 设置机器人所使用的负载ID
static PyObject* robot_set_payload_id(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_ROBOT_PAYLOAD m;
    if (!PyArg_ParseTuple(o, "ii", &m.robot_id, &m.id))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

// 设置ID对应的具体负载参数
static PyObject* set_payload(pyCommandChannel* s, PyObject* o)
{
    ZUC_TRAJ_SET_PAYLOAD m;
    if (!PyArg_ParseTuple(o, "id(ddd)", &m.id, &m.payload.m, &m.payload.p[0], &m.payload.p[1], &m.payload.p[2]))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* robot_servo_home(pyCommandChannel* s, PyObject* o)
{
    ZUC_SERVO_HOME m;
    if (!PyArg_ParseTuple(o, "ii", &m.robot_id, &m.joint))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* robot_clear_error(pyCommandChannel* s, PyObject* o)
{
    ZUC_CLEAR_ERROR m;
    if (!PyArg_ParseTuple(o, "i", &m.robot_id))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* robot_set_base_offset(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_BASE_OFFSET m;
    if (!PyArg_ParseTuple(
            o, "idddddd", &m.robot_id, &m.base_offset[0], &m.base_offset[1], &m.base_offset[2], &m.base_offset[3], &m.base_offset[4], &m.base_offset[5]))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* robot_jog_multi_joint(pyCommandChannel* s, PyObject* o)
{
    int robid = -1;
    double vel;
    RobJointVal pos;

    if (!PyArg_ParseTuple(o, "id(ddddddd)", &robid, &vel, &pos[0], &pos[1], &pos[2], &pos[3], &pos[4], &pos[5], &pos[6]))
    {
        return NULL;
    }
    printf("robid:%d\n", robid);

    ZUC_JOG_MULTI_JOINT jog_multi;
    jog_multi.robot_id = robid;
    jog_multi.vel = vel;
    jog_multi.pos = pos;
    zucSendCommand(s, jog_multi);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* robot_set_joint_min_limit(pyCommandChannel* s, PyObject* o)
{
    ZUC_JOINT_SET_MIN_POSITION_LIMIT m;
    if (!PyArg_ParseTuple(o, "iid", &m.robot_id, &m.joint, &m.limit))
        return NULL;
    // robot_set_joint_min_limit_form_jkzuc(m.joint, m.limit);
    zucSendCommand(s, m);

    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* robot_set_joint_max_limit(pyCommandChannel* s, PyObject* o)
{
    ZUC_JOINT_SET_MAX_POSITION_LIMIT m;
    if (!PyArg_ParseTuple(o, "iid", &m.robot_id, &m.joint, &m.limit))
        return NULL;
    // robot_set_joint_max_limit_form_jkzuc(m.joint, m.limit);
    zucSendCommand(s, m);

    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_admit_opt(pyCommandChannel* s, PyObject* o)
{
    ZUC_TRAJ_SET_ADMITTANCE_OPTION m;
    if (!PyArg_ParseTuple(o,
                          "i(iiiiii)",
                          &m.robot_id,
                          &m.admit_axis_opt[0],
                          &m.admit_axis_opt[1],
                          &m.admit_axis_opt[2],
                          &m.admit_axis_opt[3],
                          &m.admit_axis_opt[4],
                          &m.admit_axis_opt[5]))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_admittance_ctrl_frame(pyCommandChannel* s, PyObject* o)
{
    ZUC_TRAJ_SET_ADMITTANCE_FRAME m;
    if (!PyArg_ParseTuple(o, "ii", &m.robot_id, &m.ftFrame))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_servoj_filter(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_SERVOJ_PARAMETER m;
    PyObject* filterSetting;
    if (!PyArg_ParseTuple(o, "ii|O", &m.robot_id, &m.servo_filter_type, &filterSetting))
    {
        return NULL;
    }
    if (m.servo_filter_type == 1)
    {
        if (!PyArg_ParseTuple(filterSetting, "d", &m.lpf_cutoff_freq))
        {
            printf("invalid low pass filter parameters\n");
            return NULL;
        }
    }
    else if (m.servo_filter_type == 2)
    {
        if (!PyArg_ParseTuple(filterSetting, "ddd", &m.nlf_vr, &m.nlf_ar, &m.nlf_jr))
        {
            printf("invalid non-linear filter parameters\n");
            return NULL;
        }
    }
    else if (m.servo_filter_type == 3)
    {
        if (!PyArg_ParseTuple(filterSetting, "iddd", &m.mmf_buf_size, &m.mmf_kp, &m.mmf_kv, &m.mmf_ka))
        {
            printf("invalid multi-order mean value filter parameters\n");
            return NULL;
        }
    }
    else if (m.servo_filter_type == 4)
    {
        if (!PyArg_ParseTuple(filterSetting, "dddddd", &m.nlf_vp, &m.nlf_ap, &m.nlf_jp, &m.nlf_vr, &m.nlf_ar, &m.nlf_jr))
        {
            printf("invalid non-linear filter parameters\n");
            return NULL;
        }
    }
    else if (m.servo_filter_type == 5)
    {
        if (!PyArg_ParseTuple(filterSetting, "id", &m.mmf_buf_size, &m.mmf_kp))
        {
            printf("invalid lookahead velocity smooth parameters\n");
            return NULL;
        }
    }
    else if (m.servo_filter_type != 0)
    {
        printf("unsupported filter type\n");
        return NULL;
    }
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_admitCtrl_config(pyCommandChannel* s, PyObject* o)
{
    ZUC_TRAJ_SET_ADMITTANCE_CONFIG m;

    int axis, enableFt, enableNTrack, now;
    double ftUser, ftReboundFK, ftConst;
    if (!PyArg_ParseTuple(o, "ii(idddi)i", &m.robot_id, &axis, &enableFt, &ftUser, &ftReboundFK, &ftConst, &enableNTrack, &now))
        return NULL;

    m.axis = axis;
    m.now = now;
    m.ftConfig.enableFt = enableFt;
    m.ftConfig.enableNTrack = enableNTrack;
    m.ftConfig.ftUser = ftUser;
    m.ftConfig.ftReboundFK = ftReboundFK;
    m.ftConfig.ftConstant = ftConst;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_end_force_condition(pyCommandChannel* s, PyObject* o)
{
    ZUC_TRAJ_SET_END_FORCE_CONDITION m;

    int lowFlag, upFlag, axis, enableEndCond;
    double upLimitForce, lowLimitForce;
    if (!PyArg_ParseTuple(o, "iiiidid", &m.robot_id, &enableEndCond, &axis, &lowFlag, &lowLimitForce, &upFlag, &upLimitForce))
        return NULL;

    m.axis = axis;
    m.endForceCond.enableEndCond = enableEndCond;
    m.endForceCond.lowFlag = lowFlag;
    m.endForceCond.upFlag = upFlag;
    m.endForceCond.lowLimitForce = lowLimitForce;
    m.endForceCond.upLimitForce = upLimitForce;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_torque_sensor_soft_limit(pyCommandChannel* s, PyObject* o)
{
    ZUC_TORQUE_SENSOR_SOFT_LIMIT m;
    if (!PyArg_ParseTuple(o, "i(dddddd)", &m.robot_id, &m.Fx, &m.Fy, &m.Fz, &m.Mx, &m.My, &m.Mz))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_admittance_drag_dead_zone(pyCommandChannel* s, PyObject* o)
{
    ZUC_ADMITTANCE_DRAG_DEAD_ZONE m;
    if (!PyArg_ParseTuple(o, "i(dddddd)", &m.robot_id, &m.Fx, &m.Fy, &m.Fz, &m.Mx, &m.My, &m.Mz))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_torque_sensor_filter(pyCommandChannel* s, PyObject* o)
{
    ZUC_TORQUE_SENSOR_FILTER m;
    if (!PyArg_ParseTuple(o, "id", &m.robot_id, &m.torqueSensorFilter))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_home_vel(pyCommandChannel* s, PyObject* o)
{
    ZUC_MOTION_SET_HOME_VEL m;
    double homeVel;
    if (!PyArg_ParseTuple(o, "id", &m.robot_id, &homeVel))
        return NULL;
    m.setFlag = 1;
    m.homeVel = homeVel;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_collision_sensitivity(pyCommandChannel* s, PyObject* o)
{
    ZUC_COLLISION_SENSITIVITY m;

    if (!PyArg_ParseTuple(o,
                          "iiiiiiii",
                          &m.robot_id,
                          &m.joint_sensitivity[0],
                          &m.joint_sensitivity[1],
                          &m.joint_sensitivity[2],
                          &m.joint_sensitivity[3],
                          &m.joint_sensitivity[4],
                          &m.joint_sensitivity[5],
                          &m.joint_sensitivity[6]))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_force_limit_value(pyCommandChannel* s, PyObject* o)
{
    ZUC_FORCE_LIMIT_VALUE m;
    if (!PyArg_ParseTuple(o, "ii", &m.robot_id, &m.force_limit_value))
        return NULL;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_collision_detection_enable(pyCommandChannel* s, PyObject* o)
{
    ZUC_COLLISION_DETECTION_ENABLE m;
    if (!PyArg_ParseTuple(o, "ii", &m.robot_id, &m.collision_detection_enable))
        return NULL;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* jog_movl(pyCommandChannel* s, PyObject* o)
{
    // int iIncrFlag = 0;
    ZUC_JOG_MOVL movlCmd;
    movlCmd.tol = 0;
    movlCmd.executing_line_id = 0;
    movlCmd.end_cond[0] = -1;
    movlCmd.end_cond[1] = -1;
    movlCmd.end_cond[2] = -1;
    movlCmd.ori_vel = 180.0;
    movlCmd.ori_acc = 720.0;
    if (!PyArg_ParseTuple(o,
                          "ii(dddddd)dd|di(iii)dd",
                          &movlCmd.robot_id,
                          &movlCmd.mvOpt,
                          &movlCmd.pos_cmd[0],
                          &movlCmd.pos_cmd[1],
                          &movlCmd.pos_cmd[2],
                          &movlCmd.pos_cmd[3],
                          &movlCmd.pos_cmd[4],
                          &movlCmd.pos_cmd[5],
                          &movlCmd.vel,
                          &movlCmd.acc,
                          &movlCmd.tol,
                          &movlCmd.executing_line_id,
                          &movlCmd.end_cond[0],
                          &movlCmd.end_cond[1],
                          &movlCmd.end_cond[2],
                          &movlCmd.ori_vel,
                          &movlCmd.ori_acc))
    {
        return NULL;
    }
    if (PyTuple_Size(o) < 5)
    {
        PyErr_Format(PyExc_TypeError, "jog_movl(...) takes at least 9 arguments (%lu given)", (unsigned long)PyTuple_Size(o));
        return NULL;
    }

    zucSendCommand(s, movlCmd);
    Py_INCREF(Py_None);
    return Py_None;
}

// mvOpt: 0 for ABS
static PyObject* jog_movj(pyCommandChannel* s, PyObject* o)
{
    // int iIncrFlag = 0;
    ZUC_JOG_MOVJ movjCmd;
    movjCmd.tol = 0;
    movjCmd.executing_line_id = 0;
    movjCmd.end_cond[0] = -1;
    movjCmd.end_cond[1] = -1;
    movjCmd.end_cond[2] = -1;
    movjCmd.is_cpos = 0;
    movjCmd.robot_id = 0;

    if (!PyArg_ParseTuple(o,
                          "ii(ddddddd)dd|di(iii)",
                          &movjCmd.robot_id,
                          &movjCmd.mvOpt,
                          &movjCmd.pos_cmd[0],
                          &movjCmd.pos_cmd[1],
                          &movjCmd.pos_cmd[2],
                          &movjCmd.pos_cmd[3],
                          &movjCmd.pos_cmd[4],
                          &movjCmd.pos_cmd[5],
                          &movjCmd.pos_cmd[6],
                          &movjCmd.vel,
                          &movjCmd.acc,
                          &movjCmd.tol,
                          &movjCmd.executing_line_id,
                          &movjCmd.end_cond[0],
                          &movjCmd.end_cond[1],
                          &movjCmd.end_cond[2]))
    {
        return NULL;
    }
    if (PyTuple_Size(o) < 5)
    {
        PyErr_Format(PyExc_TypeError, "jog_movj(...) takes at least 10 arguments (%lu given)", (unsigned long)PyTuple_Size(o));
        return NULL;
    }

    zucSendCommand(s, movjCmd);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* jog_movjp(pyCommandChannel* s, PyObject* o)
{
    // int iIncrFlag = 0;
    ZUC_JOG_MOVJ movjCmd;
    movjCmd.tol = 0;
    movjCmd.executing_line_id = 0;
    movjCmd.end_cond[0] = -1;
    movjCmd.end_cond[1] = -1;
    movjCmd.end_cond[2] = -1;
    movjCmd.is_cpos = 1;

    if (!PyArg_ParseTuple(o,
                          "iddddddddd|di(iii)",
                          &movjCmd.mvOpt,
                          &movjCmd.pos_cmd[0],
                          &movjCmd.pos_cmd[1],
                          &movjCmd.pos_cmd[2],
                          &movjCmd.pos_cmd[3],
                          &movjCmd.pos_cmd[4],
                          &movjCmd.pos_cmd[5],
                          &movjCmd.pos_cmd[6],
                          &movjCmd.vel,
                          &movjCmd.acc,
                          &movjCmd.tol,
                          &movjCmd.executing_line_id,
                          &movjCmd.end_cond[0],
                          &movjCmd.end_cond[1],
                          &movjCmd.end_cond[2]))
    {
        return NULL;
    }

    if (PyTuple_Size(o) < 10)
    {
        PyErr_Format(PyExc_TypeError, "jog_movjp(...) takes at least 9 arguments (%lu given)", (unsigned long)PyTuple_Size(o));
        return NULL;
    }

    zucSendCommand(s, movjCmd);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* jog_movc(pyCommandChannel* s, PyObject* o)
{
    ZUC_JOG_MOVC m;
    m.tol = 0;
    m.circle_cnt = 0;
    m.executing_line_id = 0;
    m.end_cond[0] = -1;
    m.end_cond[1] = -1;
    m.end_cond[2] = -1;
    m.circlemode = 0;  // @FIXME liwang 172 circlemode 未定义
    if (!PyArg_ParseTuple(o,
                          "i(dddddd)(dddddd)dd|di(iii)ii",
                          &m.mvOpt,
                          &m.pos_mid[0],
                          &m.pos_mid[1],
                          &m.pos_mid[2],
                          &m.pos_mid[3],
                          &m.pos_mid[4],
                          &m.pos_mid[5],
                          &m.pos_end[0],
                          &m.pos_end[1],
                          &m.pos_end[2],
                          &m.pos_end[3],
                          &m.pos_end[4],
                          &m.pos_end[5],
                          &m.vel,
                          &m.acc,
                          &m.tol,
                          &m.executing_line_id,
                          &m.end_cond[0],
                          &m.end_cond[1],
                          &m.end_cond[2],
                          &m.circle_cnt,
                          &m.circlemode))
    {
        return NULL;
    }
    if (PyTuple_Size(o) < 5)
    {
        PyErr_Format(PyExc_TypeError, "jog_movc(...) takes at least 5 arguments (%lu given)", (unsigned long)PyTuple_Size(o));
        return NULL;
    }

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* servo_upgrade(pyCommandChannel* s, PyObject* o)
{
    ZUC_SERVO_UPGRADE upgrade;
    zucSendCommand(s, upgrade);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* getAllVersion(pyCommandChannel* s, PyObject* o)
{
    // ZUC_SERVO_UPGRADE upgrade;
    // upgrade.intid = 0;
    // if(!PyArg_ParseTuple(o, "i", &upgrade.intid)) {
    //     return NULL;
    // }
    // if(upgrade.intid == 0)//一键升级的必然不为0
    // {
    //     return NULL;
    // }
    // zucSendCommand(s,upgrade);
    // Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* servoj(pyCommandChannel* s, PyObject* o)
{
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* servop(pyCommandChannel* s, PyObject* o)
{
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* reset_interpreter(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_PLAN_INIT m;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* program_open(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_PLAN_CLOSE m0;
    zucSendCommand(s, m0);
    ZUC_TASK_PLAN_OPEN m;
    char* file;
    Py_ssize_t len;

    if (!PyArg_ParseTuple(o, "s#", &file, &len))
        return NULL;
    if (unsigned(len) > sizeof(m.file) - 1)
    {
        PyErr_Format(PyExc_ValueError, "File name limited to %zu characters", sizeof(m.file) - 1);
        return NULL;
    }
    strcpy(m.file, file);
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* zucauto(pyCommandChannel* s, PyObject* o)
{
    int fn;
    ZUC_TASK_PLAN_RUN run;
    ZUC_TASK_PLAN_PAUSE pause;
    ZUC_TASK_PLAN_RESUME resume;
    ZUC_TASK_PLAN_STEP step;

    if (PyArg_ParseTuple(o, "ii", &fn, &run.line) && fn == LOCAL_AUTO_RUN)
    {
        zucSendCommand(s, run);
    }
    else
    {
        PyErr_Clear();
        if (!PyArg_ParseTuple(o, "i", &fn))
            return NULL;
        switch (fn)
        {
        case LOCAL_AUTO_PAUSE:
            zucSendCommand(s, pause);
            break;
        case LOCAL_AUTO_RESUME:
            zucSendCommand(s, resume);
            break;
        case LOCAL_AUTO_STEP:
            zucSendCommand(s, step);
            break;
        default:
            PyErr_Format(Exception_GetBaseError(), "Unexpected argument '%d' to command.auto", fn);
            return NULL;
        }
    }
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* config_profinet(pyCommandChannel* s, PyObject* o)
{
    int enable;
    if (!PyArg_ParseTuple(o, "i", &enable))
        return NULL;
    printf("config_profinet %d\n", enable);
    std::string filename = ZUC_DEFAULT_ROOT_DIR "/configs/JAKA/usersettings.ini";
    // boost::property_tree::ptree m_pt, tag_setting;
    // read_ini(filename, m_pt);
    // m_pt.put<int>("PROFINET.ENABLE", enable);
    // write_ini(filename, m_pt);
    Util::IniParser ini_file(filename.c_str());
    ini_file.set("PROFINET.ENABLE", enable);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* Set_eip(pyCommandChannel* s, PyObject* o)
{
    int enable;
    if (!PyArg_ParseTuple(o, "i", &enable))
        return NULL;
    std::string filename = ZUC_DEFAULT_ROOT_DIR "/configs/JAKA/usersettings.ini";
    // boost::property_tree::ptree m_pt, tag_setting;
    // read_ini(filename, m_pt);
    // m_pt.put<int>("EIP.ENABLE", enable);
    // write_ini(filename, m_pt);
    Util::IniParser ini_file(filename.c_str());
    ini_file.set("EIP.ENABLE", enable);
    Py_INCREF(Py_None);
    return Py_None;
}

#define CLEAR_PFN_FUNIO(FUNCNAME)                                                               \
    do {                                                                                        \
        IOTypeIndex* fun = &funio.FUNCNAME;                                                     \
        if (fun->type == 0 && fun->index >= pfnid && fun->index < pfnid + ZUCMOT_MAX_PNDev_DIO) \
        {                                                                                       \
            fun->index = -1;                                                                    \
            fun->type = -1;                                                                     \
        }                                                                                       \
    } while (0)

static PyObject* clear_profinet_funcio(pyCommandChannel* s, PyObject* o)
{
    ZUC_STAT* stat = (ZUC_STAT*)s->s->get_address();
    FuncIOMap funio = stat->io.funcIOMap;

    int pfnid = ZUCMOT_ZLAN_DIO_NUM + ZUCMOT_MAX_MODBUS_DIO;
    if (stat->cab_type == 2)
    {
        pfnid = ZUCMOT_SCB_USER_DIO_NUM + ZUCMOT_MAX_MODBUS_DIO;
    }
    else if (stat->cab_type == 3)
    {
        pfnid = ZUCMOT_MINI_USER_DIO_NUM + ZUCMOT_MAX_MODBUS_DIO;
    }
    CLEAR_PFN_FUNIO(task_run_di);
    CLEAR_PFN_FUNIO(task_pause_di);
    CLEAR_PFN_FUNIO(task_resume_di);
    CLEAR_PFN_FUNIO(task_stop_di);
    CLEAR_PFN_FUNIO(power_on_di);
    CLEAR_PFN_FUNIO(power_off_di);
    CLEAR_PFN_FUNIO(servo_enable_di);
    CLEAR_PFN_FUNIO(servo_disable_di);
    CLEAR_PFN_FUNIO(back_to_inipos_di);
    CLEAR_PFN_FUNIO(stop_mode_di);
    CLEAR_PFN_FUNIO(percentage_mode_level1_di);
    CLEAR_PFN_FUNIO(percentage_mode_level2_di);
    CLEAR_PFN_FUNIO(clear_error_di);
    CLEAR_PFN_FUNIO(drag_mode_di);
    CLEAR_PFN_FUNIO(drag_mode_disable_di);
    for (int i = 0; i < 4; i++)
    {
        CLEAR_PFN_FUNIO(task_idle_do[i]);
        CLEAR_PFN_FUNIO(task_paused_do[i]);
        CLEAR_PFN_FUNIO(task_running_do[i]);
        CLEAR_PFN_FUNIO(stat_fault_do[i]);
        CLEAR_PFN_FUNIO(stat_poweron_do[i]);
        CLEAR_PFN_FUNIO(stat_enabled_do[i]);
        CLEAR_PFN_FUNIO(stat_motion_do[i]);
        CLEAR_PFN_FUNIO(stat_static_do[i]);
        CLEAR_PFN_FUNIO(stat_boot_do[i]);
        CLEAR_PFN_FUNIO(stat_emergency_stop_do[i]);
        CLEAR_PFN_FUNIO(stat_percentage_mode_level2_do[i]);
        CLEAR_PFN_FUNIO(stat_sys_protective_stop_do[i]);
        CLEAR_PFN_FUNIO(stat_initial_pose_do[i]);
        CLEAR_PFN_FUNIO(stat_percentage_mode_level1_do[i]);
        CLEAR_PFN_FUNIO(stat_drag_mode_do[i]);
        CLEAR_PFN_FUNIO(stat_collision_mode_do[i]);
        CLEAR_PFN_FUNIO(stat_manual_mode_do[i]);
        CLEAR_PFN_FUNIO(stat_auto_mode_do[i]);
    }
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* setCabVoltage(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_CABVOLTAGE vol;

    if (!PyArg_ParseTuple(o, "i", &vol.cabVoltage))
        return NULL;
    zucSendCommand(s, vol);

    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* setAppConnectionStatus(pyCommandChannel* s, PyObject* o)
{
    ZUC_APP_SET_CONNECTSTAT cmd;

    if (!PyArg_ParseTuple(o, "i", &cmd.status))
        return NULL;
    zucSendCommand(s, cmd);

    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* debug(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_DEBUG d;

    if (!PyArg_ParseTuple(o, "i", &d.debug))
        return NULL;
    zucSendCommand(s, d);

    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* teleop(pyCommandChannel* s, PyObject* o)
{
    ZUC_TRAJ_SET_TELEOP_ENABLE en;

    if (!PyArg_ParseTuple(o, "i", &en.enable))
        return NULL;

    zucSendCommand(s, en);

    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* teleop_tool(pyCommandChannel* s, PyObject* o)
{
    ZUC_TRAJ_SET_TELEOP_TOOL_ENABLE en;

    if (!PyArg_ParseTuple(o, "i", &en.enable))
        return NULL;

    zucSendCommand(s, en);

    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* servojop(pyCommandChannel* s, PyObject* o)
{
    ZUC_TRAJ_SET_SERVOJOP_ENABLE en;
    if (!PyArg_ParseTuple(o, "ii", &en.robot_id, &en.enable))
        return NULL;

    zucSendCommand(s, en);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* admitop(pyCommandChannel* s, PyObject* o)
{
    ZUC_TRAJ_SET_ADMITOP_ENABLE en;

    if (!PyArg_ParseTuple(o, "i", &en.enable))
        return NULL;

    zucSendCommand(s, en);

    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_adaptive_feed(pyCommandChannel* s, PyObject* o)
{
    ZUC_MOTION_ADAPTIVE m;
    if (!PyArg_ParseTuple(o, "b", &m.status))
        return NULL;

    zucSendCommand(s, m);

    Py_INCREF(Py_None);
    return Py_None;
}
//
static PyObject* set_digital_output(pyCommandChannel* s, PyObject* o)
{
    ZUC_MOTION_SET_DOUT m;
    if (!PyArg_ParseTuple(o, "bib", &m.type, &m.index, &m.start))
        return NULL;

    m.now = 1;
    zucSendCommand(s, m);

    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_analog_output(pyCommandChannel* s, PyObject* o)
{
    ZUC_MOTION_SET_AOUT m;
    if (!PyArg_ParseTuple(o, "bbd", &m.type, &m.index, &m.start))
        return NULL;

    m.now = 1;
    zucSendCommand(s, m);

    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_io_type(pyCommandChannel* s, PyObject* o)
{
    // if (!PyArg_ParseTuple(o, "i", &ioType))
    //     return NULL;

    // Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* exit_protect(pyCommandChannel* s, PyObject* o)
{
    ZUC_MOTION_EXIT_PROTECT exit_prot;
    if (!PyArg_ParseTuple(o, "i", &exit_prot.protect_type))
        return NULL;
    if (exit_prot.protect_type < 0 || exit_prot.protect_type > 2)
        return NULL;
    zucSendCommand(s, exit_prot);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_clsn_sensi(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_SERVO_PARAM servo_param_msg;
    servo_param_msg.servoParam.paramType = ServoParam::CLSN_SENSITIVITY;
    if (!PyArg_ParseTuple(o, "ii", &servo_param_msg.robot_id, &servo_param_msg.servoParam.paramVal.clsn_sensitivity))
        return NULL;
    zucSendCommand(s, servo_param_msg);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_brake_release(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_SERVO_PARAM servo_param_msg;
    servo_param_msg.servoParam.paramType = ServoParam::BRAKE_CRTL_PARAM;
    if (!PyArg_ParseTuple(o,
                          "iii",
                          &servo_param_msg.robot_id,
                          &servo_param_msg.servoParam.paramVal.brakeCmd.jointId,
                          &servo_param_msg.servoParam.paramVal.brakeCmd.releaseBrake))
        return NULL;
    zucSendCommand(s, servo_param_msg);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_multiturn_step(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_SERVO_PARAM servo_param_msg;
    servo_param_msg.servoParam.paramType = ServoParam::MULTI_TURN_STEP;
    if (!PyArg_ParseTuple(o,
                          "iii",
                          &servo_param_msg.robot_id,
                          &servo_param_msg.servoParam.paramVal.multiTurnCmd.jointId,
                          &servo_param_msg.servoParam.paramVal.multiTurnCmd.multiTurnStep))
        return NULL;
    zucSendCommand(s, servo_param_msg);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_torque_forward_enable(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_SERVO_PARAM servo_param_msg;
    servo_param_msg.servoParam.paramType = ServoParam::TORQUE_FORWARD_ENA;
    if (!PyArg_ParseTuple(o, "ii", &servo_param_msg.robot_id, &servo_param_msg.servoParam.paramVal.torque_forward_ena))
        return NULL;
    zucSendCommand(s, servo_param_msg);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_servo_param(pyCommandChannel* s, PyObject* o)
{
    ZUC_SET_SERVO_PARAM servo_param_msg;
    servo_param_msg.servoParam.paramType = ServoParam::GENERIC_CMD;
    if (!PyArg_ParseTuple(o,
                          "iiii",
                          &servo_param_msg.robot_id,
                          &servo_param_msg.servoParam.paramVal.geneCmd.jointId,
                          &servo_param_msg.servoParam.paramVal.geneCmd.paramId,
                          &servo_param_msg.servoParam.paramVal.geneCmd.value))
        return NULL;
    zucSendCommand(s, servo_param_msg);
    Py_INCREF(Py_None);
    return Py_None;
}

static void get_robot_param(pyCommandChannel* s, RobotSettings* robot_config)
{
    if (s == NULL || robot_config == NULL)
    {
        return;
    }
    s->s->peek();
    ZUC_STAT* stat = (ZUC_STAT*)s->s->get_address();
    robot_config->robot_fulldhcom = stat->motion.full_dh_com;
    printf(" ################################################ [get_robot_param] robot_fulldhcom = %d ################################################\n",
           robot_config->robot_fulldhcom);
    // robot series
    robot_config->robot_series = stat->motion.robot_serial_num / 100000000;
    //dh_para
    int robot_joint_num = stat->motion.robot_joint_num;
    for (int i = 0; i < robot_joint_num; i++)
    {
        robot_config->dh[i] = stat->motion.dhParam.d[i];
        robot_config->dh[robot_joint_num + i] = stat->motion.dhParam.a[i];
        robot_config->dh[robot_joint_num * 2 + i] = stat->motion.dhParam.joint_homeoff[i];
        robot_config->dh[robot_joint_num * 3 + i] = stat->motion.dhParam.alpha[i];
        // printf("d[%d] = %f\t", i,stat->motion.dhParam.d[i] );
        // printf("a[%d] = %f\t", i,stat->motion.dhParam.a[i] );
        // printf("theta[%d] = %f\t", i,stat->motion.dhParam.joint_homeoff[i] );
        // printf("alpha[%d] = %f\n", i,stat->motion.dhParam.alpha[i] );
    }

    //base_offset
    auto base_id = stat->motion.base_frame_id;
    robot_config->base_offset[0] = stat->userOffset[base_id].a;
    robot_config->base_offset[1] = stat->userOffset[base_id].b;
    robot_config->base_offset[2] = stat->userOffset[base_id].c;

    //user_offset
    robot_config->user_offset[0] = stat->task.robotUserOffset.tran.x;
    robot_config->user_offset[1] = stat->task.robotUserOffset.tran.y;
    robot_config->user_offset[2] = stat->task.robotUserOffset.tran.z;
    robot_config->user_offset[3] = stat->task.robotUserOffset.a;
    robot_config->user_offset[4] = stat->task.robotUserOffset.b;
    robot_config->user_offset[5] = stat->task.robotUserOffset.c;

    //tool_offset
    robot_config->tool_offset[0] = stat->task.robotToolOffset.tran.x;
    robot_config->tool_offset[1] = stat->task.robotToolOffset.tran.y;
    robot_config->tool_offset[2] = stat->task.robotToolOffset.tran.z;
    robot_config->tool_offset[3] = stat->task.robotToolOffset.a;
    robot_config->tool_offset[4] = stat->task.robotToolOffset.b;
    robot_config->tool_offset[5] = stat->task.robotToolOffset.c;

    // robot_config->joint_offset = stat->motion.robotJointOffset;
    memcpy(robot_config->joint_offset, stat->motion.robotJointOffset, sizeof(double) * ZUCMOT_MAX_JOINTS);
}

static PyObject* kine_inverse(pyCommandChannel* s, PyObject* o)
{
    ZUC_MOTION_KINE_INVERSE m;
    if (!PyArg_ParseTuple(o,
                          "i(dddddd)(ddddddd)",
                          &m.robindex,
                          &m.tarPose.tran.x,
                          &m.tarPose.tran.y,
                          &m.tarPose.tran.z,
                          &m.tarPose.a,
                          &m.tarPose.b,
                          &m.tarPose.c,
                          &m.currJoint[0],
                          &m.currJoint[1],
                          &m.currJoint[2],
                          &m.currJoint[3],
                          &m.currJoint[4],
                          &m.currJoint[5],
                          &m.currJoint[6]))
        return NULL;
    zucSendCommand(s, m);
    esleep(0.1);
    PyObject* res = PyTuple_New(8);
    ZUC_STAT* stat = (ZUC_STAT*)s->s->get_address();
    PyTuple_SetItem(res, 0, PyLong_FromLong(stat->usrOpeErrCode));
    for (int i = 0; i < 7; i++) { PyTuple_SetItem(res, i + 1, PyFloat_FromDouble(stat->motion.utilJointPos[i])); }
    return res;
}

static PyObject* kine_inverse_all(pyCommandChannel* s, PyObject* o)
{
    Py_INCREF(Py_None);
    return Py_None;
    // RobotSettings robotCfg;
    // get_robot_param(s, &robotCfg);
    // kin_set_robot_series(robotCfg.robot_series);
    // kin_set_full_dh_com(robotCfg.robot_fulldhcom);
    // kin_set_dh_param(robotCfg.dh);
    // kin_set_base_offset(robotCfg.base_offset);
    // kin_set_user_offset(robotCfg.user_offset);
    // kin_set_tool_offset(robotCfg.tool_offset);

    // double cartpos[6] = {0, 0, 0, 0, 0, 0};
    // double transpose[6] = {0};
    // double currJoint[6] = {0};

    // if (!PyArg_ParseTuple(o,
    //                       "(dddddd)(dddddd)",
    //                       &transpose[0],
    //                       &transpose[1],
    //                       &transpose[2],
    //                       &transpose[3],
    //                       &transpose[4],
    //                       &transpose[5],
    //                       &currJoint[0],
    //                       &currJoint[1],
    //                       &currJoint[2],
    //                       &currJoint[3],
    //                       &currJoint[4],
    //                       &currJoint[5]))
    //     return NULL;

    // // printf("transpose is : %f, %f, %f, %f, %f, %f\n", transpose[0], transpose[1], transpose[2], transpose[3], transpose[4], transpose[5]);
    // // printf("currJoint is : %f, %f, %f, %f, %f, %f\n", currJoint[0], currJoint[1], currJoint[2], currJoint[3], currJoint[4], currJoint[5]);
    // int ret_num = 0;
    // double qInv[8][6] = {0.0};
    // // @shixiaoli 待库更新后恢复
    // // kin_inverse_all(transpose, currJoint, qInv, &ret_num);
    // // printf("inverse result %d: %f, %f, %f, %f, %f, %f\n", result, currJoint[0], currJoint[1], currJoint[2], currJoint[3], currJoint[4], currJoint[5]);
    // if (ret_num == 0)
    // {
    //     printf("singular pose\n");
    // }
    // PyObject* inverse_result = PyDict_New();

    // for (int num = 0; num < ret_num; num++)
    // {
    //     PyObject* inverse_jpos = PyTuple_New(6);
    //     for (int i = 0; i < 6; i++) { PyTuple_SetItem(inverse_jpos, i, PyFloat_FromDouble(qInv[num][i])); }
    //     switch (num)
    //     {
    //     case 0:
    //         PyDict_SetItemString(inverse_result, "inverse_jpos1", inverse_jpos);
    //         break;
    //     case 1:
    //         PyDict_SetItemString(inverse_result, "inverse_jpos2", inverse_jpos);
    //         break;
    //     case 2:
    //         PyDict_SetItemString(inverse_result, "inverse_jpos3", inverse_jpos);
    //         break;
    //     case 3:
    //         PyDict_SetItemString(inverse_result, "inverse_jpos4", inverse_jpos);
    //         break;
    //     case 4:
    //         PyDict_SetItemString(inverse_result, "inverse_jpos5", inverse_jpos);
    //         break;
    //     case 5:
    //         PyDict_SetItemString(inverse_result, "inverse_jpos6", inverse_jpos);
    //         break;
    //     case 6:
    //         PyDict_SetItemString(inverse_result, "inverse_jpos7", inverse_jpos);
    //         break;
    //     case 7:
    //         PyDict_SetItemString(inverse_result, "inverse_jpos8", inverse_jpos);
    //         break;
    //     }
    // }
    // PyDict_SetItemString(inverse_result, "inverse_res_num", PyLong_FromLong(ret_num));
    // return inverse_result;
}

static PyObject* kine_forward(pyCommandChannel* s, PyObject* o)
{
    ZUC_MOTION_KINE_FORWARD m;
    ZUC_STAT* stat = (ZUC_STAT*)s->s->get_address();
    int robindex = 0;
    if (!PyArg_ParseTuple(o, "i(ddddddd)", &m.robindex, &m.joints[0], &m.joints[1], &m.joints[2], &m.joints[3], &m.joints[4], &m.joints[5], &m.joints[6]))
        return NULL;
    zucSendCommand(s, m);
    esleep(0.1);
    PyObject* res = PyTuple_New(7);
    PyTuple_SetItem(res, 0, PyLong_FromLong(stat->usrOpeErrCode));

    PyTuple_SetItem(res, 1, PyFloat_FromDouble(stat->motion.utilCartPos.tran.x));
    PyTuple_SetItem(res, 2, PyFloat_FromDouble(stat->motion.utilCartPos.tran.y));
    PyTuple_SetItem(res, 3, PyFloat_FromDouble(stat->motion.utilCartPos.tran.z));
    PyTuple_SetItem(res, 4, PyFloat_FromDouble(stat->motion.utilCartPos.a));
    PyTuple_SetItem(res, 5, PyFloat_FromDouble(stat->motion.utilCartPos.b));
    PyTuple_SetItem(res, 6, PyFloat_FromDouble(stat->motion.utilCartPos.c));

    return res;
}

static PyObject* calc_tcp(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_CALC_TCP m;
    if (!PyArg_ParseTuple(o,
                          "iii(ddddddd)(ddddddd)(ddddddd)(ddddddd)",
                          &m.id,
                          &m.saveType,
                          &m.statusFlag,
                          &m.jntpoint1[0],
                          &m.jntpoint1[1],
                          &m.jntpoint1[2],
                          &m.jntpoint1[3],
                          &m.jntpoint1[4],
                          &m.jntpoint1[5],
                          &m.jntpoint1[6],
                          &m.jntpoint2[0],
                          &m.jntpoint2[1],
                          &m.jntpoint2[2],
                          &m.jntpoint2[3],
                          &m.jntpoint2[4],
                          &m.jntpoint2[5],
                          &m.jntpoint2[6],
                          &m.jntpoint3[0],
                          &m.jntpoint3[1],
                          &m.jntpoint3[2],
                          &m.jntpoint3[3],
                          &m.jntpoint3[4],
                          &m.jntpoint3[5],
                          &m.jntpoint3[6],
                          &m.jntpoint4[0],
                          &m.jntpoint4[1],
                          &m.jntpoint4[2],
                          &m.jntpoint4[3],
                          &m.jntpoint4[4],
                          &m.jntpoint4[5],
                          &m.jntpoint4[6]))
        return NULL;

    zucSendCommand(s, m);
    esleep(0.1);

    /*retrieve the result tool offset*/
    PyObject* res = PyTuple_New(10);
    ZUC_STAT* stat = (ZUC_STAT*)s->s->get_address();
    PyTuple_SetItem(res, 0, PyLong_FromLong(stat->usrOpeErrCode));
    PyTuple_SetItem(res, 1, PyFloat_FromDouble(stat->motion.utilCartPos.tran.x));
    PyTuple_SetItem(res, 2, PyFloat_FromDouble(stat->motion.utilCartPos.tran.y));
    PyTuple_SetItem(res, 3, PyFloat_FromDouble(stat->motion.utilCartPos.tran.z));
    PyTuple_SetItem(res, 4, PyFloat_FromDouble(stat->motion.utilCartPos.a));
    PyTuple_SetItem(res, 5, PyFloat_FromDouble(stat->motion.utilCartPos.b));
    PyTuple_SetItem(res, 6, PyFloat_FromDouble(stat->motion.utilCartPos.c));
    PyTuple_SetItem(res, 7, PyFloat_FromDouble(stat->motion.tcpError[0]));
    PyTuple_SetItem(res, 8, PyFloat_FromDouble(stat->motion.tcpError[1]));
    PyTuple_SetItem(res, 9, PyFloat_FromDouble(stat->motion.tcpError[2]));

    // TODO: rcs_print存在segmentation fault问题，暂用printf替代
    printf("cal_tcp result: %d, %f, %f, %f, %f, %f, %f, %f, %f, %f\n",
           stat->usrOpeErrCode,
           stat->motion.utilCartPos.tran.x,
           stat->motion.utilCartPos.tran.y,
           stat->motion.utilCartPos.tran.z,
           stat->motion.utilCartPos.a,
           stat->motion.utilCartPos.b,
           stat->motion.utilCartPos.c,
           stat->motion.tcpError[0],
           stat->motion.tcpError[1],
           stat->motion.tcpError[2]);
    return res;
}

static PyObject* calc_tcp_six(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_CALC_TCP m;
    m.methodtype = 1;
    if (!PyArg_ParseTuple(o,
                          "iii(ddddddd)(ddddddd)(ddddddd)(ddddddd)(ddddddd)(ddddddd)",
                          &m.id,
                          &m.saveType,
                          &m.statusFlag,
                          &m.jntpoint1[0],
                          &m.jntpoint1[1],
                          &m.jntpoint1[2],
                          &m.jntpoint1[3],
                          &m.jntpoint1[4],
                          &m.jntpoint1[5],
                          &m.jntpoint1[6],
                          &m.jntpoint2[0],
                          &m.jntpoint2[1],
                          &m.jntpoint2[2],
                          &m.jntpoint2[3],
                          &m.jntpoint2[4],
                          &m.jntpoint2[5],
                          &m.jntpoint2[6],
                          &m.jntpoint3[0],
                          &m.jntpoint3[1],
                          &m.jntpoint3[2],
                          &m.jntpoint3[3],
                          &m.jntpoint3[4],
                          &m.jntpoint3[5],
                          &m.jntpoint3[6],
                          &m.jntpoint4[0],
                          &m.jntpoint4[1],
                          &m.jntpoint4[2],
                          &m.jntpoint4[3],
                          &m.jntpoint4[4],
                          &m.jntpoint4[5],
                          &m.jntpoint4[6],
                          &m.jntpoint5[0],
                          &m.jntpoint5[1],
                          &m.jntpoint5[2],
                          &m.jntpoint5[3],
                          &m.jntpoint5[4],
                          &m.jntpoint5[5],
                          &m.jntpoint5[6],
                          &m.jntpoint6[0],
                          &m.jntpoint6[1],
                          &m.jntpoint6[2],
                          &m.jntpoint6[3],
                          &m.jntpoint6[4],
                          &m.jntpoint6[5],
                          &m.jntpoint6[6]))
        return NULL;

    zucSendCommand(s, m);
    zucWaitCommandComplete(s, ZUC_COMMAND_TIMEOUT);

    /*retrieve the result tool offset*/
    PyObject* res = PyTuple_New(10);
    ZUC_STAT* stat = (ZUC_STAT*)s->s->get_address();
    PyTuple_SetItem(res, 0, PyLong_FromLong(stat->usrOpeErrCode));
    PyTuple_SetItem(res, 1, PyFloat_FromDouble(stat->motion.utilCartPos.tran.x));  //utilCartPos is updated in real time
    PyTuple_SetItem(res, 2, PyFloat_FromDouble(stat->motion.utilCartPos.tran.y));
    PyTuple_SetItem(res, 3, PyFloat_FromDouble(stat->motion.utilCartPos.tran.z));
    PyTuple_SetItem(res, 4, PyFloat_FromDouble(stat->motion.utilCartPos.a));
    PyTuple_SetItem(res, 5, PyFloat_FromDouble(stat->motion.utilCartPos.b));
    PyTuple_SetItem(res, 6, PyFloat_FromDouble(stat->motion.utilCartPos.c));
    PyTuple_SetItem(res, 7, PyFloat_FromDouble(stat->motion.tcpError[0]));
    PyTuple_SetItem(res, 8, PyFloat_FromDouble(stat->motion.tcpError[1]));
    PyTuple_SetItem(res, 9, PyFloat_FromDouble(stat->motion.tcpError[2]));

    printf("cal_tcp result: %d, %f, %f, %f, %f, %f, %f, %f, %f, %f\n",
           stat->usrOpeErrCode,
           stat->motion.utilCartPos.tran.x,
           stat->motion.utilCartPos.tran.y,
           stat->motion.utilCartPos.tran.z,
           stat->motion.utilCartPos.a,
           stat->motion.utilCartPos.b,
           stat->motion.utilCartPos.c,
           stat->motion.tcpError[0],
           stat->motion.tcpError[1],
           stat->motion.tcpError[2]);
    return res;
}

static PyObject* calc_safeatt(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_CALC_SAFEATT m;
    if (!PyArg_ParseTuple(o,
                          "(dddddd)(dddddd)(dddddd)ddi",
                          &m.cartpos1.tran.x,
                          &m.cartpos1.tran.y,
                          &m.cartpos1.tran.z,
                          &m.cartpos1.a,
                          &m.cartpos1.b,
                          &m.cartpos1.c,
                          &m.cartpos2.tran.x,
                          &m.cartpos2.tran.y,
                          &m.cartpos2.tran.z,
                          &m.cartpos2.a,
                          &m.cartpos2.b,
                          &m.cartpos2.c,
                          &m.cartpos3.tran.x,
                          &m.cartpos3.tran.y,
                          &m.cartpos3.tran.z,
                          &m.cartpos3.a,
                          &m.cartpos3.b,
                          &m.cartpos3.c,
                          &m.tilt,
                          &m.pan,
                          &m.tipChoice))
        return NULL;
    // printf("m: %f, %f, %f, %f, %f, %f\n", m.cartpos1.tran.x,m.cartpos1.tran.y,m.cartpos1.tran.z,m.cartpos1.a,m.cartpos1.b,m.cartpos1.c);
    // printf("m: %f, %f, %f, %f, %f, %f\n", m.cartpos2.tran.x,m.cartpos2.tran.y,m.cartpos2.tran.z,m.cartpos2.a,m.cartpos2.b,m.cartpos2.c);
    // printf("m: %f, %f, %f, %f, %f, %f\n", m.cartpos3.tran.x,m.cartpos3.tran.y,m.cartpos3.tran.z,m.cartpos3.a,m.cartpos3.b,m.cartpos3.c);
    // printf("m: %i, %i, %i\n", m.tilt, m.pan, m.tipChoice);

    zucSendCommand(s, m);
    esleep(0.1);

    /*retrieve the result tool offset*/
    PyObject* res = PyTuple_New(5);
    ZUC_STAT* stat = (ZUC_STAT*)s->s->get_address();
    PyTuple_SetItem(res, 0, PyLong_FromLong(stat->usrOpeErrCode));
    PyTuple_SetItem(res, 1, PyFloat_FromDouble(stat->motion.safeatt.conicalCenRpy.r));
    PyTuple_SetItem(res, 2, PyFloat_FromDouble(stat->motion.safeatt.conicalCenRpy.p));
    PyTuple_SetItem(res, 3, PyFloat_FromDouble(stat->motion.safeatt.conicalCenRpy.y));
    PyTuple_SetItem(res, 4, PyFloat_FromDouble(stat->motion.safeatt.deviation));

    // TODO: 使用rcs_print调用时，存在segmentation fault的问题，暂时用printf替换
    printf("calsafeatt result: %d,%lf, %lf, %lf, %lf\n ",
           stat->usrOpeErrCode,
           stat->motion.safeatt.conicalCenRpy.r,
           stat->motion.safeatt.conicalCenRpy.p,
           stat->motion.safeatt.conicalCenRpy.y,
           stat->motion.safeatt.deviation);
    // rcs_print("11111\n");
    return res;
}

static PyObject* calc_user(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_CALC_USER m;
    // rcs_print("---------calc_user---------\n");
    if (!PyArg_ParseTuple(o,
                          "iii(ddddddd)(ddddddd)(ddddddd)",
                          &m.id,
                          &m.saveType,
                          &m.statusFlag,
                          &m.jntpoint1[0],
                          &m.jntpoint1[1],
                          &m.jntpoint1[2],
                          &m.jntpoint1[3],
                          &m.jntpoint1[4],
                          &m.jntpoint1[5],
                          &m.jntpoint1[6],
                          &m.jntpoint2[0],
                          &m.jntpoint2[1],
                          &m.jntpoint2[2],
                          &m.jntpoint2[3],
                          &m.jntpoint2[4],
                          &m.jntpoint2[5],
                          &m.jntpoint2[6],
                          &m.jntpoint3[0],
                          &m.jntpoint3[1],
                          &m.jntpoint3[2],
                          &m.jntpoint3[3],
                          &m.jntpoint3[4],
                          &m.jntpoint3[5],
                          &m.jntpoint3[6]))
        return NULL;

    zucSendCommand(s, m);
    esleep(0.1);

    /*retrieve the result tool offset*/
    PyObject* res = PyTuple_New(7);
    ZUC_STAT* stat = (ZUC_STAT*)s->s->get_address();
    PyTuple_SetItem(res, 0, PyLong_FromLong(stat->usrOpeErrCode));
    PyTuple_SetItem(res, 1, PyFloat_FromDouble(stat->motion.utilCartPos.tran.x));
    PyTuple_SetItem(res, 2, PyFloat_FromDouble(stat->motion.utilCartPos.tran.y));
    PyTuple_SetItem(res, 3, PyFloat_FromDouble(stat->motion.utilCartPos.tran.z));
    PyTuple_SetItem(res, 4, PyFloat_FromDouble(stat->motion.utilCartPos.a));
    PyTuple_SetItem(res, 5, PyFloat_FromDouble(stat->motion.utilCartPos.b));
    PyTuple_SetItem(res, 6, PyFloat_FromDouble(stat->motion.utilCartPos.c));

    printf("cal_user result: %d, %f, %f, %f, %f, %f, %f\n",
           stat->usrOpeErrCode,
           stat->motion.utilCartPos.tran.x,
           stat->motion.utilCartPos.tran.y,
           stat->motion.utilCartPos.tran.z,
           stat->motion.utilCartPos.a,
           stat->motion.utilCartPos.b,
           stat->motion.utilCartPos.c);
    return res;
}

static PyObject* set_user_defined_variable(pyCommandChannel* s, PyObject* o)
{
    ZUC_TASK_SET_USER_DEFINED_VARIABLE m;
    if (!PyArg_ParseTuple(o, "id", &m.id, &m.value))
        return NULL;

    zucSendCommand(s, m);

    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* wait_complete(pyCommandChannel* s, PyObject* o)
{
    double timeout = ZUC_COMMAND_TIMEOUT;
    if (!PyArg_ParseTuple(o, "|d:zuc.command.wait_complete", &timeout))
        return NULL;
    return PyLong_FromLong(zucWaitCommandComplete(s, timeout));
}

typedef struct
{
    double xyz_interval;
    double rpy_interval;
    double vel;
    double acc;
} trajTrackPara;

bool get_smooth_config(int* coef, int* para)
{
    try
    {
        char filename[120] = ZUC_DEFAULT_ROOT_DIR "/configs/JAKA/usersettings.ini";
        // ZucIniFile trajInifile;
        // trajInifile.Open(filename);
        // trajInifile.Find(coef, "SMOOTH_COEF", "SMOOTH");
        // trajInifile.Find(para, "SPARSE_PARA", "SMOOTH");
        Util::IniParser ini_file(filename);
        //TODO: range check
        ini_file.get("SMOOTH.SMOOTH_COEF", *coef, 994);
        ini_file.get("SMOOTH.SPARSE_PARA", *para, 2, 1, 3);

        return true;
    }
    catch (const std::exception& e)
    {
        printf("read trajconfig error:%s", e.what());
        return false;
    }
}

// bool call_movs_lib(pyCommandChannel* s, char* txtpath, double* trajparam)
// {
//     uint16_t path_buffer_size = 128 * 2 + 64;  // 若修改 path_buff_size大小，需同步修改 RenameDirAndFile 中 len
//     char txt_path[path_buffer_size] = {0}, ngc_path[path_buffer_size] = {0}, tmp_ngc_path[path_buffer_size] = {0}, ini_path[path_buffer_size] = {0};
//     snprintf(txt_path, path_buffer_size, ZUC_DEFAULT_ROOT_DIR "/scripts/track/%s/%s.txt", txtpath, txtpath);  //  max length  of txtpath is 128 byte
//     snprintf(ngc_path, path_buffer_size, ZUC_DEFAULT_ROOT_DIR "/scripts/track/%s/%s.jks", txtpath, txtpath);
//     snprintf(tmp_ngc_path, path_buffer_size, "/tmp/%s.jks", txtpath);
//     snprintf(ini_path, path_buffer_size, ZUC_DEFAULT_ROOT_DIR "/scripts/track/%s/subtrajconfig.ini", txtpath);

//     RobotSettings robotCfg;
//     get_robot_param(s, &robotCfg);
//     synch_robot_settings(&robotCfg);
//     try
//     {
//         std::ifstream infile(txt_path);
//         if (!infile.is_open())
//         {
//             return false;
//         }
//         int line_num = 0;
//         std::string temp;
//         while (getline(infile, temp))
//         {
//             line_num++;
//             if (line_num > 10)
//                 break;  // 避免记录的轨迹较长时，消耗过多时间。
//         }

//         if (line_num <= 10)
//         {
//             try
//             {
//                 remove(txt_path);
//                 snprintf(txt_path, path_buffer_size, ZUC_DEFAULT_ROOT_DIR "/scripts/track/%s/subtrajconfig.ini", txtpath);
//                 remove(txt_path);
//                 snprintf(txt_path, path_buffer_size, ZUC_DEFAULT_ROOT_DIR "/scripts/track/%s", txtpath);
//                 rmdir(txt_path);
//             }
//             catch (...)
//             {
//                 printf("remove directory error:%s\n", txt_path);
//             }
//             return false;
//         }
//         printf("libsmooth version:%s\n", get_version().c_str());
//         printf("%f,%f-----------------\n", trajparam[2], trajparam[3]);

//         double coef;
//         int para, mid_coef;
//         if (!get_smooth_config(&mid_coef, &para))
//         {
//             printf("-------------------------------------------------------\n");
//             coef = 0.994;
//             para = 2;
//         }
//         else
//         {
//             int mid = mid_coef;
//             int cnt = 0;
//             while (mid != 0)
//             {
//                 mid = mid / 10;
//                 cnt++;
//             }
//             coef = mid_coef / pow(10, cnt);
//         }

//         try
//         {
//             if (generate_traj_script_joint(txt_path, tmp_ngc_path, trajparam[2], trajparam[3], coef, para) == 1)
//                 return false;
//         }
//         catch (...)
//         {
//             remove(txt_path);
//             remove(ini_path);
//             sprintf(txt_path, ZUC_DEFAULT_ROOT_DIR "/scripts/track/%s", txtpath);
//             rmdir(txt_path);
//             printf("libsmooth called failed\n");
//             return false;
//         }

//         // s->s->peek();
//         // ZUC_STAT *stat = (ZUC_STAT *)s->s->get_address();
//         // double ref_joint_pos[ZUCMOT_MAX_JOINTS];
//         // ref_joint_pos[0] = stat->motion.joint[0].output;
//         // ref_joint_pos[1] = stat->motion.joint[1].output;
//         // ref_joint_pos[2] = stat->motion.joint[2].output;
//         // ref_joint_pos[3] = stat->motion.joint[3].output;
//         // ref_joint_pos[4] = stat->motion.joint[4].output;
//         // ref_joint_pos[5] = stat->motion.joint[5].output;
//         // if (generate_traj_script(txt_path, tmp_ngc_path, trajparam[2], trajparam[3], 0.994, 2, ref_joint_pos) == 1)
//         //     return false;
//     }
//     catch (const std::exception& e)
//     {
//         return false;
//     }

//     //add tool set to ngc
//     std::ofstream infile(ngc_path);
//     infile << "endPosJ =[0,0,0,0,0,0]"
//            << "\n"
//            << "endPosL =[0,0,0,0,0,0]"
//            << "\n"
//            << "pos_mvl =[0,0,0,0,0,0]"
//            << "\n"
//            << "pos_waypoint =[0,0,0,0,0,0]"
//            << "\n";
//     infile << "toolpos =[";
//     char tool_info[200] = {0};
//     sprintf(tool_info,
//             "%f,%f,%f,%f,%f,%f}\n",
//             robotCfg.tool_offset[0],
//             robotCfg.tool_offset[1],
//             robotCfg.tool_offset[2],
//             robotCfg.tool_offset[3],
//             robotCfg.tool_offset[4],
//             robotCfg.tool_offset[5]);
//     infile << tool_info;
//     infile << "set_tool(toolpos)"
//            << "\n";

//     std::ifstream midfile(tmp_ngc_path);
//     std::string temp;
//     while (getline(midfile, temp)) { infile << temp << "\n"; }
//     infile.close();
//     midfile.close();
//     return true;
// }

void get_global_traj_config(double* trajconfig)
{
    try
    {
        std::string filename = ZUC_DEFAULT_ROOT_DIR "/configs/JAKA/trajconfig.ini";
        // boost::property_tree::ptree m_pt, tag_setting;
        // read_ini(filename, m_pt);

        // tag_setting = m_pt.get_child("INTERVAL");
        // trajconfig[0] = tag_setting.get<double>("xyz_interval");
        // trajconfig[1] = tag_setting.get<double>("rpy_interval");

        // tag_setting = m_pt.get_child("VEL");
        // trajconfig[2] = tag_setting.get<double>("vel");

        // tag_setting = m_pt.get_child("ACC");
        // trajconfig[3] = tag_setting.get<double>("acc");
        Util::IniParser ini_file(filename.c_str());
        ini_file.get("INTERVAL.xyz_interval", trajconfig[0], 2.0, 0.1, 10);
        ini_file.get("INTERVAL.rpy_interval", trajconfig[1], 2.0, 0.1, 10);
        ini_file.get("VEL.vel", trajconfig[2], 500.0, 0.01, 1800);
        ini_file.get("ACC.acc", trajconfig[3], 200.0, 0.01, 3500);
    }
    catch (const std::exception& e)
    {
        printf("read trajconfig error:%s", e.what());
    }
}

bool get_sub_traj_config(double* trajconfig, char* dirname)
{
    try
    {
        char filename[256] = {0};
        sprintf(filename, ZUC_DEFAULT_ROOT_DIR "/scripts/track/%s/subtrajconfig.ini", dirname);
        // boost::property_tree::ptree m_pt, tag_setting;
        // read_ini(filename, m_pt);

        // tag_setting = m_pt.get_child("VEL");
        // trajconfig[2] = tag_setting.get<double>("vel");

        // tag_setting = m_pt.get_child("ACC");
        // trajconfig[3] = tag_setting.get<double>("acc");
        Util::IniParser ini_file(filename);
        ini_file.get("VEL.vel", trajconfig[2], 500.0, 0.01, 1800);
        ini_file.get("ACC.acc", trajconfig[3], 200.0, 0.01, 3500);
        return true;
    }
    catch (const std::exception& e)
    {
        printf("read trajconfig error:%s", e.what());
        return false;
    }
}

static PyObject* generate_ngc_file(pyCommandChannel* s, PyObject* o)
{
    // char* name;
    // trajTrackPara m;
    // if (!PyArg_ParseTuple(o, "s(dddd)", &name, &m.xyz_interval, &m.rpy_interval, &m.vel, &m.acc))
    //     return NULL;
    // if (strlen(name) > 128)
    //     return NULL;
    // double set[4];
    // //判断是否使用轨迹复现全局默认配置参数
    // char filename[256] = {0};
    // snprintf(filename, 256, ZUC_DEFAULT_ROOT_DIR "/scripts/track/%s/subtrajconfig.ini", name);
    // std::ifstream fp;
    // fp.open(filename);
    // if (fp)
    // {
    //     printf("get_sub_traj_config is called-------------------------\n");
    //     get_sub_traj_config(set, name);
    // }
    // else
    // {
    //     printf("get_global_traj_config is called--------------------\n");
    //     get_global_traj_config(set);
    // }
    // // get_global_traj_config(set);
    // bool result;
    // result = call_movs_lib(s, name, set);

    // PyObject* res = PyTuple_New(1);
    // PyTuple_SetItem(res, 0, PyLong_FromLong(result));
    // return res;

    Py_INCREF(Py_None);
    return Py_None;
}

std::atomic<bool> sampling(false);
char traj_sample_data_filename[110] = {0};
char traj_data_global_filename[120] = {0};
double oldJointValue[ZUCMOT_MAX_JOINTS] = {0};

// @FIXME liwang 7 轴适配
bool sample_process(pyCommandChannel* s)
{
    return false;

    // char txt_file[120] = {0};

    // if (traj_sample_data_filename[0] == '\0')
    // {
    //     struct std::tm* ptr;
    //     time_t timestamp;
    //     timestamp = time(NULL);
    //     ptr = localtime(&timestamp);
    //     strftime(txt_file, 100, "%Y-%m-%d-%H-%M-%S", ptr);
    //     strftime(traj_data_global_filename, 100, "%Y-%m-%d-%H-%M-%S", ptr);
    // }
    // else
    // {
    //     memcpy(txt_file, traj_sample_data_filename, 100);
    //     memcpy(traj_data_global_filename, traj_sample_data_filename, 100);
    //     traj_sample_data_filename[0] = '\0';
    // }

    // char txt_dir[150] = {0};
    // sprintf(txt_dir, ZUC_DEFAULT_ROOT_DIR "/scripts/track/%s", txt_file);

    // if (-1 == mkdir(txt_dir, S_IRWXO | S_IRWXU | S_IRWXG))  //-1: failed  0:successed
    // {
    //     printf("mkdir %s error (errno = %d)\n", txt_dir, errno);
    //     return false;
    // }

    // usleep(10000);
    // chmod(txt_dir, S_IRWXU);
    // usleep(10000);

    // // create subtrajconfig.ini
    // char sub_traj_config_filename[120] = {0};
    // sprintf(sub_traj_config_filename, "%s/subtrajconfig.ini", txt_dir);
    // try
    // {
    //     double trajconfig[4];
    //     get_global_traj_config(trajconfig);
    //     Util::IniParser ini_file(sub_traj_config_filename);
    //     ini_file.set("VEL.vel", trajconfig[2]);
    //     ini_file.set("ACC.acc", trajconfig[3]);
    //     // boost::property_tree::ptree m_pt, tag_setting;
    //     // char tmp[30];
    //     // sprintf(tmp, "%3.2f", trajconfig[2]);
    //     // m_pt.put<std::string>("VEL.vel", tmp);
    //     // sprintf(tmp, "%3.2f", trajconfig[3]);
    //     // m_pt.put<std::string>("ACC.acc", tmp);
    //     // boost::property_tree::ini_parser::write_ini(sub_traj_config_filename, m_pt);
    // }
    // catch (const std::exception& e)
    // {
    //     std::cerr << e.what() << '\n';
    //     return false;
    // }

    // char txt_path[200] = {0};
    // sprintf(txt_path, "%s/%s.txt", txt_dir, txt_file);

    // std::ofstream fs;
    // fs.open(txt_path, std::ios::out);
    // if (!fs.is_open())
    // {
    //     printf("trajectory_teach can not creat file");
    //     return false;
    // }

    // s->s->peek();
    // ZUC_STAT* stat = (ZUC_STAT*)s->s->get_address();
    // int linenum = 0;
    // try
    // {
    //     double forwardpos[CART_DIM] = {0}, currentpos[CART_DIM] = {0}, trajconfig[4] = {0}, currentjointpos[ZUCMOT_MAX_JOINTS] = {0};
    //     get_global_traj_config(trajconfig);
    //     if (abs(trajconfig[0]) > 10.0)
    //         trajconfig[0] = 10.0;
    //     if (abs(trajconfig[1]) > 10.0)
    //         trajconfig[1] = 10.0;

    //     if (abs(trajconfig[0]) < 0.1)
    //         trajconfig[0] = 0.1;
    //     if (abs(trajconfig[1]) < 0.1)
    //         trajconfig[1] = 0.1;

    //     // int sample_data_line = 0;
    //     while (sampling)
    //     {
    //         // ++sample_data_line;
    //         usleep(5000);
    //         s->s->peek();
    //         stat = (ZUC_STAT*)s->s->get_address();
    //         currentpos[0] = stat->motion.traj.actualPosition.tran.x;
    //         currentpos[1] = stat->motion.traj.actualPosition.tran.y;
    //         currentpos[2] = stat->motion.traj.actualPosition.tran.z;
    //         currentpos[3] = stat->motion.traj.actualPosition.a;
    //         currentpos[4] = stat->motion.traj.actualPosition.b;
    //         currentpos[5] = stat->motion.traj.actualPosition.c;

    //         for (int i = 0; i < stat->motion.robot_joint_num; i++) { currentjointpos[i] = stat->motion.joint[i].output; }

    //         double xyz_chg = sqrt(pow(currentpos[0], 2) + pow(currentpos[1], 2) + pow(currentpos[2], 2)) -
    //                          sqrt(pow(forwardpos[0], 2) + pow(forwardpos[1], 2) + pow(forwardpos[2], 2));
    //         double rpy_chg = sqrt(pow(currentpos[3], 2) + pow(currentpos[4], 2) + pow(currentpos[5], 2)) -
    //                          sqrt(pow(forwardpos[3], 2) + pow(forwardpos[4], 2) + pow(forwardpos[5], 2));

    //         if ((abs(xyz_chg) > trajconfig[0]) || (abs(rpy_chg) > trajconfig[1]))
    //         {
    //             bool jointValueChg = false;
    //             for (int i = 0; i < stat->motion.robot_joint_num; ++i)
    //             {
    //                 if (abs(currentjointpos[i] - oldJointValue[i]) > 0.05)
    //                     jointValueChg = true;
    //                 oldJointValue[i] = currentjointpos[i];
    //             }
    //             if (jointValueChg)
    //             {
    //                 char pos[150] = {0};
    //                 // sprintf(pos, "%f %f %f %f %f %f %f %f %f %f %f %f\n", currentpos[0], currentpos[1], currentpos[2]
    //                 //                                                 , currentpos[3], currentpos[4], currentpos[5]
    //                 //                                                 , currentjointpos[0], currentjointpos[1], currentjointpos[2]
    //                 //                                                 , currentjointpos[3], currentjointpos[4], currentjointpos[5]);
    //                 sprintf(pos,
    //                         "%f %f %f %f %f %f %f %f %f %f %f %f\n",
    //                         currentjointpos[0],
    //                         currentjointpos[1],
    //                         currentjointpos[2],
    //                         currentjointpos[3],
    //                         currentjointpos[4],
    //                         currentjointpos[5],
    //                         currentpos[0],
    //                         currentpos[1],
    //                         currentpos[2],
    //                         currentpos[3],
    //                         currentpos[4],
    //                         currentpos[5]);
    //                 fs << pos;
    //                 linenum++;
    //                 for (int i = 0; i < CART_DIM; ++i) { forwardpos[i] = currentpos[i]; }
    //             }
    //         }

    //         if (linenum > 100000)
    //         {
    //             sampling = false;
    //         }
    //     }
    //     fs.close();
    //     sampling = false;
    //     if (linenum > 100000)
    //     {
    //         sleep(1.5);
    //         if (call_movs_lib(s, txt_file, trajconfig) == 1)
    //             return false;
    //     }
    // }
    // catch (const std::exception& e)
    // {
    //     printf("trajectory_teach error:%s", e.what());
    // }
    // return true;
}

static PyObject* set_traj_sample_data_filename(pyCommandChannel* s, PyObject* o)
{
    char* file;
    if (!PyArg_ParseTuple(o, "s", &file))
        return NULL;
    memcpy(traj_sample_data_filename, file, 100);

    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* traj_sample(pyCommandChannel* s, PyObject* o)
{
    int trajsample;
    PyObject* res = PyTuple_New(1);

    if (!PyArg_ParseTuple(o, "i", &trajsample))
        return NULL;

    if (!trajsample)
    {
        sampling = false;

        PyTuple_SetItem(res, 0, PyUnicode_FromString(traj_data_global_filename));
        traj_data_global_filename[0] = '\0';
        return res;
    }

    if (sampling)
    {
        PyTuple_SetItem(res, 0, PyLong_FromLong(0));
        return res;
    }
    sampling = true;
    std::thread f(sample_process, s);

    f.detach();

    PyTuple_SetItem(res, 0, PyLong_FromLong(1));
    return res;
}

static PyObject* get_traj_sample_status(pyCommandChannel* s)
{
    PyObject* res = PyLong_FromLong(sampling);
    return res;
}

static PyObject* set_capture(pyCommandChannel* s, PyObject* o)
{
    ZUC_MOTION_SET_CAPTURE captCmd;
    int ioType;  // 0: cabinet IO, 1: tool IO, 2: ext IO
    int ioIndex;
    int trigEvent;  // 0: falling edge, 1: rising edge, 2: low level, 3: high level

    if (!PyArg_ParseTuple(o, "ii(iii)", &captCmd.captureId, &captCmd.stateToCapt, &ioType, &ioIndex, &trigEvent))
        return NULL;

    captCmd.trigger.ioType = ioType;
    captCmd.trigger.ioIndex = ioIndex;
    captCmd.trigger.trigEvent = trigEvent;

    zucSendCommand(s, captCmd);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_auto_op(pyCommandChannel* s, PyObject* o)
{
    ZUC_MOTION_ONE_KEY_OP m;

    // if(!PyArg_ParseTuple(o, "i", &m.auto_op_setting)) return NULL;
    if (!PyArg_ParseTuple(o, "ii", &m.onekeyenable, &m.autoprogram))
        return NULL;
    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* clear_capture(pyCommandChannel* s, PyObject* o)
{
    ZUC_MOTION_CLEAR_CAPTURE clrCaptCmd;
    if (!PyArg_ParseTuple(o, "i", &clrCaptCmd.captureId))
        return NULL;

    zucSendCommand(s, clrCaptCmd);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* diag_collect_data(pyCommandChannel* s, PyObject* o)
{
    ZUC_COLLECT_DIAG_DATA diag;
    if (!PyArg_ParseTuple(o, "i", &diag.isStart))
        return NULL;
    zucSendCommand(s, diag);

    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* get_serial(pyCommandChannel* s) { return PyLong_FromLong(s->serial); }

static PyObject* set_full_dh_flag(pyCommandChannel* s, PyObject* o)
{
    Robot_SET_FULL_DH_FLAG r;
    int full_dh_flag = 0;
    if (!PyArg_ParseTuple(o, "i", &full_dh_flag))
    {
        return NULL;
    }
    // kin_set_full_dh_com(full_dh_flag);
    r.full_dh_flag = full_dh_flag;
    zucSendCommand(s, r);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_7_dof_avoid_limit_param_alpha_k(pyCommandChannel* s, PyObject* o)
{
    Robot_SET_7_DOF_KINE_PARAM_ALPHA_K r;
    double k;  // 0: 臂角固定且根据当前机器人关节角状态计算 1: 臂角固定且根据输入arm_angle计算 2: 臂型角随动，根据当前臂型角为起点
    double alpha;
    if (!PyArg_ParseTuple(o, "dd", &k, &alpha))
    {
        return NULL;
    }
    r.k = k;
    r.alpha = alpha;
    // kin_set_joint_lim_Algo_param(k, alpha);
    zucSendCommand(s, r);
    Py_INCREF(Py_None);
    return Py_None;
}

static int torqueShmKey = 206;
static TorqueControlShm* torqueControlShmStruct = NULL;
static int tor_control_id = 0;

static PyObject* set_torque_control_enable(pyCommandChannel* s, PyObject* o)
{
    ZUC_TORQUE_CONTROL_ENABLE m;
    if (!PyArg_ParseTuple(o, "ii", &m.torque_control_enable, &m.torque_last_period))
        return NULL;
    if (m.torque_control_enable < 0 || m.torque_control_enable > 1 || m.torque_last_period < 0)
    {
        return NULL;
    }

    if (torqueControlShmStruct == NULL)
    {
        tor_control_id = shmget(torqueShmKey, sizeof(TorqueControlShm), IPC_CREAT | 0666);
        if (tor_control_id < 0)
        {
            printf("access super shared memory error 1\n");
            return Py_None;
        }
        torqueControlShmStruct = (TorqueControlShm*)shmat(tor_control_id, NULL, 0);
        if (torqueControlShmStruct == NULL)
        {
            printf("access super shared memory error 2\n");
            return Py_None;
        }
    }

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_drag_control_enable(pyCommandChannel* s, PyObject* o)
{
    ZUC_DRAG_CONTROL_ENABLE m;
    if (!PyArg_ParseTuple(o, "i", &m.drag_control_enable))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_drag_control_fc_compensate(pyCommandChannel* s, PyObject* o)
{
    ZUC_DRAG_CONTROL_FC_COMPENSATE m;
    if (!PyArg_ParseTuple(o,
                          "iiiiiii",
                          &m.drag_control_fc_compensate[0],
                          &m.drag_control_fc_compensate[1],
                          &m.drag_control_fc_compensate[2],
                          &m.drag_control_fc_compensate[3],
                          &m.drag_control_fc_compensate[4],
                          &m.drag_control_fc_compensate[5],
                          &m.drag_control_fc_compensate[6]))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_drag_control_fv_compensate(pyCommandChannel* s, PyObject* o)
{
    ZUC_DRAG_CONTROL_FV_COMPENSATE m;
    if (!PyArg_ParseTuple(o,
                          "iiiiiii",
                          &m.drag_control_fv_compensate[0],
                          &m.drag_control_fv_compensate[1],
                          &m.drag_control_fv_compensate[2],
                          &m.drag_control_fv_compensate[3],
                          &m.drag_control_fv_compensate[4],
                          &m.drag_control_fv_compensate[5],
                          &m.drag_control_fv_compensate[6]))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_torque_control_value(pyCommandChannel* s, PyObject* o)
{
    int res = -1;
    double torque_value[ZUCMOT_MAX_JOINTS];
    if (!PyArg_ParseTuple(
            o, "ddddddd", &torque_value[0], &torque_value[1], &torque_value[2], &torque_value[3], &torque_value[4], &torque_value[5], &torque_value[6]))
        return NULL;
    TorqueControlVal var;
    for (int i = 0; i < ZUCMOT_MAX_JOINTS; i++) { var.torque[i] = torque_value[i]; }
    if (torqueControlShmStruct)
    {
        tor_control_id = sem_get(TORQUE_CONTROL_SEM_KEY);
        if (sem_p(tor_control_id) == 0)
        {
            if (!torqueControlShmStruct->enable)
            {
                rcs_print("Torque Control Must Be enbaled fisrt!\n");
                res = -1;
            }
            else
            {
                if ((torqueControlShmStruct->torque_que.rear + 1) % MAX_TORQUE_CONTROL_QUE_LEN == torqueControlShmStruct->torque_que.front)
                {
                    res = -1;
                }
                else
                {
                    if (torqueControlShmStruct->torque_que.front == -1)
                        torqueControlShmStruct->torque_que.front = 0;
                    torqueControlShmStruct->torque_que.rear = (torqueControlShmStruct->torque_que.rear + 1) % MAX_TORQUE_CONTROL_QUE_LEN;
                    torqueControlShmStruct->torque_que.torque_que[torqueControlShmStruct->torque_que.rear] = var;
                    res = ((torqueControlShmStruct->torque_que.rear + MAX_TORQUE_CONTROL_QUE_LEN - torqueControlShmStruct->torque_que.front) %
                           MAX_TORQUE_CONTROL_QUE_LEN) +
                          1;
                }
            }
            sem_v(tor_control_id);
        }
    }
    return Py_BuildValue("i", res);
}

static PyObject* set_gravity_direction(pyCommandChannel* s, PyObject* o)
{
    ZUC_GRAVITY_DIRECTION m;
    if (!PyArg_ParseTuple(o, "ddd", &m.rpy[0], &m.rpy[1], &m.rpy[2]))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_drag_control_filter_factor(pyCommandChannel* s, PyObject* o)
{
    ZUC_DRAG_CONTROL_FILTER_FACTOR m;
    if (!PyArg_ParseTuple(o,
                          "iiiiiii",
                          &m.drag_control_filter_factor[0],
                          &m.drag_control_filter_factor[1],
                          &m.drag_control_filter_factor[2],
                          &m.drag_control_filter_factor[3],
                          &m.drag_control_filter_factor[4],
                          &m.drag_control_filter_factor[5],
                          &m.drag_control_filter_factor[6]))
    {
        return NULL;
    }

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject* set_drag_control_dither_factor(pyCommandChannel* s, PyObject* o)
{
    ZUC_DRAG_CONTROL_DITHER_FACTOR m;
    if (!PyArg_ParseTuple(o,
                          "ddddddd",
                          &m.drag_control_dither_factor[0],
                          &m.drag_control_dither_factor[1],
                          &m.drag_control_dither_factor[2],
                          &m.drag_control_dither_factor[3],
                          &m.drag_control_dither_factor[4],
                          &m.drag_control_dither_factor[5],
                          &m.drag_control_dither_factor[6]))
        return NULL;

    zucSendCommand(s, m);
    Py_INCREF(Py_None);
    return Py_None;
}

static PyGetSetDef Command_getset[] = {{(char*)"serial", (getter)get_serial}, {NULL}};

static PyMethodDef Command_methods[] = {{"setCabVoltage", (PyCFunction)setCabVoltage, METH_VARARGS},
                                        {"debug", (PyCFunction)debug, METH_VARARGS},
                                        {"set_app_connection_status", (PyCFunction)setAppConnectionStatus, METH_VARARGS},
                                        {"teleop_enable", (PyCFunction)teleop, METH_VARARGS},
                                        {"teleop_tool_enable", (PyCFunction)teleop_tool, METH_VARARGS}, /*added by Zhenxiang Qi*/
                                        {"servojop_enable", (PyCFunction)servojop, METH_VARARGS},
                                        {"admitop_enable", (PyCFunction)admitop, METH_VARARGS},
                                        {"wait_complete", (PyCFunction)wait_complete, METH_VARARGS},
                                        {"scb_power_on", (PyCFunction)scb_power_on, METH_VARARGS},
                                        {"scb_power_off", (PyCFunction)scb_power_off, METH_VARARGS},
                                        {"rob_enable", (PyCFunction)rob_enable, METH_VARARGS},
                                        {"rob_disable", (PyCFunction)rob_disable, METH_VARARGS},
                                        {"mdi", (PyCFunction)mdi, METH_VARARGS},
                                        {"mode", (PyCFunction)mode, METH_VARARGS},
                                        {"setOperationMode", (PyCFunction)setOperationMode, METH_VARARGS},
                                        {"feedrate", (PyCFunction)feedrate, METH_VARARGS},
                                        {"rapidrate", (PyCFunction)rapidrate, METH_VARARGS},
                                        {"maxvel", (PyCFunction)maxvel, METH_VARARGS},
                                        {"spindle", (PyCFunction)spindle, METH_VARARGS},
                                        {"set_tool_offset", (PyCFunction)set_tool_offset, METH_VARARGS},
                                        {"set_user_frame", (PyCFunction)set_user_frame, METH_VARARGS},
                                        {"set_installation_angle", (PyCFunction)set_installation_angle, METH_VARARGS},
                                        {"set_payload", (PyCFunction)set_payload, METH_VARARGS},
                                        {"calib_dhparam", (PyCFunction)calib_dhparam, METH_VARARGS},
                                        {"set_funcdi", (PyCFunction)set_funcdi, METH_VARARGS},
                                        {"set_funcdo", (PyCFunction)set_funcdo, METH_VARARGS},
                                        {"set_safety_funcdi", (PyCFunction)set_safety_funcdi, METH_VARARGS},
                                        {"set_safety_funcdo", (PyCFunction)set_safety_funcdo, METH_VARARGS},
                                        {"mist", (PyCFunction)mist, METH_VARARGS},
                                        {"flood", (PyCFunction)flood, METH_VARARGS},
                                        {"brake", (PyCFunction)brake, METH_VARARGS},
                                        {"load_tool_table", (PyCFunction)load_tool_table, METH_NOARGS},
                                        {"abort", (PyCFunction)zucabort, METH_NOARGS},
                                        {"task_plan_synch", (PyCFunction)task_plan_synch, METH_NOARGS},
                                        // {"override_limits", (PyCFunction)override_limits, METH_NOARGS},
                                        // {"home", (PyCFunction)home, METH_VARARGS},
                                        // {"unhome", (PyCFunction)unhome, METH_VARARGS},
                                        {"jog",
                                         (PyCFunction)jog,
                                         METH_VARARGS,
                                         "jog(JOG_CONTINUOUS, joint_flag, index, speed)\n"
                                         "jog(JOG_INCREMENT, joint_flag, index, speed, increment)\n"
                                         "jog(JOG_STOP, joint_flag, index)\n"
                                         "\n"
                                         "Start or stop a continuous or incremental jog of a joint or an axis.\n"
                                         "\n"
                                         "    joint_flag: True to jog a joint, False to jog an axis\n"
                                         "    index: the index of the joint or axis to jog\n"
                                         "    speed: jog speed\n"
                                         "    increment: distance to jog\n"},
                                        {"jog_movj", (PyCFunction)jog_movj, METH_VARARGS},
                                        {"jog_movl", (PyCFunction)jog_movl, METH_VARARGS},
                                        {"jog_movjp", (PyCFunction)jog_movjp, METH_VARARGS},
                                        {"jog_movc", (PyCFunction)jog_movc, METH_VARARGS},
                                        {"servoj", (PyCFunction)servoj, METH_VARARGS},
                                        {"servop", (PyCFunction)servop, METH_VARARGS},
                                        {"reset_interpreter", (PyCFunction)reset_interpreter, METH_NOARGS},
                                        {"program_open", (PyCFunction)program_open, METH_VARARGS},
                                        {"auto", (PyCFunction)zucauto, METH_VARARGS},
                                        {"set_optional_stop", (PyCFunction)optional_stop, METH_VARARGS},
                                        {"set_block_delete", (PyCFunction)block_delete, METH_VARARGS},
                                        {"robot_set_joint_min_limit", (PyCFunction)robot_set_joint_min_limit, METH_VARARGS},
                                        {"robot_set_joint_max_limit", (PyCFunction)robot_set_joint_max_limit, METH_VARARGS},
                                        {"set_adaptive_feed", (PyCFunction)set_adaptive_feed, METH_VARARGS},
                                        {"set_digital_output", (PyCFunction)set_digital_output, METH_VARARGS},
                                        {"set_analog_output", (PyCFunction)set_analog_output, METH_VARARGS},
                                        {"set_io_type", (PyCFunction)set_io_type, METH_VARARGS},
                                        {"exit_protect", (PyCFunction)exit_protect, METH_VARARGS},
                                        {"set_clsn_sensi", (PyCFunction)set_clsn_sensi, METH_VARARGS},
                                        {"set_brake_release", (PyCFunction)set_brake_release, METH_VARARGS},
                                        {"set_multiturn_step", (PyCFunction)set_multiturn_step, METH_VARARGS},
                                        {"set_torque_forward_enable", (PyCFunction)set_torque_forward_enable, METH_VARARGS},
                                        {"set_servo_param", (PyCFunction)set_servo_param, METH_VARARGS},
                                        {"kine_inverse", (PyCFunction)kine_inverse, METH_VARARGS},
                                        {"kine_inverse_all", (PyCFunction)kine_inverse_all, METH_VARARGS},
                                        {"kine_forward", (PyCFunction)kine_forward, METH_VARARGS},
                                        {"calc_tcp", (PyCFunction)calc_tcp, METH_VARARGS},
                                        {"calc_safeatt", (PyCFunction)calc_safeatt, METH_VARARGS},
                                        {"set_safe_attitude", (PyCFunction)set_safe_attitude, METH_VARARGS},
                                        {"set_planner_jerk", (PyCFunction)set_planner_jerk, METH_VARARGS},
                                        {"calc_tcp_six", (PyCFunction)calc_tcp_six, METH_VARARGS},
                                        {"calc_user", (PyCFunction)calc_user, METH_VARARGS},
                                        {"set_user_defined_variable", (PyCFunction)set_user_defined_variable, METH_VARARGS},
                                        {"set_muti_tool_offsets", (PyCFunction)set_muti_tool_offsets, METH_VARARGS},
                                        {"set_muti_user_offsets", (PyCFunction)set_muti_user_offsets, METH_VARARGS},
                                        {"set_safe_zone", (PyCFunction)set_safe_zone, METH_VARARGS},
                                        {"world_cartesian_point_convert_to_user", (PyCFunction)world_cartesian_point_convert_to_user, METH_VARARGS},
                                        {"user_cartesian_point_convert_to_world", (PyCFunction)user_cartesian_point_convert_to_world, METH_VARARGS},
                                        {"set_tool_id", (PyCFunction)set_tool_id, METH_VARARGS},
                                        {"set_user_id", (PyCFunction)set_user_id, METH_VARARGS},
                                        {"set_auto_work", (PyCFunction)set_auto_work, METH_VARARGS},
                                        {"set_auto_op", (PyCFunction)set_auto_op, METH_VARARGS},
                                        {"set_robot_name", (PyCFunction)set_robot_name, METH_VARARGS},
                                        {"set_cab_id", (PyCFunction)set_cab_id, METH_VARARGS},
                                        {"set_script_backup", (PyCFunction)set_script_backup, METH_VARARGS},
                                        {"setup_mb_slave", (PyCFunction)setup_mb_slave, METH_VARARGS},
                                        {"servo_upgrade", (PyCFunction)servo_upgrade, METH_NOARGS},
                                        {"getAllVersion", (PyCFunction)getAllVersion, METH_NOARGS},
                                        {"setup_extio", (PyCFunction)setup_extio, METH_VARARGS},
                                        {"set_extio_mode", (PyCFunction)set_extio_mode, METH_VARARGS},
                                        {"set_maintenance_mode", (PyCFunction)set_maintenance_mode, METH_VARARGS},
                                        {"set_torqsensor_comm", (PyCFunction)set_torqsensor_comm, METH_VARARGS},
                                        {"set_torqsensor_payload", (PyCFunction)set_torqsensor_payload, METH_VARARGS},
                                        {"set_tool_payload", (PyCFunction)set_tool_payload, METH_VARARGS},
                                        {"set_torqsensor_mode", (PyCFunction)set_torqsensor_mode, METH_VARARGS},
                                        {"set_torqsensor_brand", (PyCFunction)set_torqsensor_brand, METH_VARARGS},
                                        {"set_admittance_ctrl_frame", (PyCFunction)set_admittance_ctrl_frame, METH_VARARGS},
                                        {"disable_force_control", (PyCFunction)disable_force_control, METH_VARARGS},
                                        {"set_admit_opt", (PyCFunction)set_admit_opt, METH_VARARGS},
                                        {"set_custom_pose", (PyCFunction)set_custom_pose, METH_VARARGS},
                                        {"set_identify_traj_startpos", (PyCFunction)set_identify_traj_startpos, METH_VARARGS},
                                        {"set_identify_traj_endpos", (PyCFunction)set_identify_traj_endpos, METH_VARARGS},
                                        {"start_identify_traj", (PyCFunction)start_identify_traj, METH_VARARGS},
                                        {"get_identify_result", (PyCFunction)get_identify_result, METH_VARARGS},
                                        {"get_payload_auto_identify_result", (PyCFunction)get_payload_auto_identify_result, METH_VARARGS},
                                        {"reset_identify_status", (PyCFunction)reset_identify_status, METH_VARARGS},
                                        {"set_percentage_mode_level_value", (PyCFunction)set_percentage_mode_level_value, METH_VARARGS},
                                        {"set_percentage_mode", (PyCFunction)set_percentage_mode, METH_VARARGS},
                                        {"set_torque_sensor_soft_limit", (PyCFunction)set_torque_sensor_soft_limit, METH_VARARGS},
                                        {"set_admittance_drag_dead_zone", (PyCFunction)set_admittance_drag_dead_zone, METH_VARARGS},
                                        {"set_torque_sensor_filter", (PyCFunction)set_torque_sensor_filter, METH_VARARGS},
                                        {"set_admitCtrl_config", (PyCFunction)set_admitCtrl_config, METH_VARARGS},
                                        {"set_end_force_condition", (PyCFunction)set_end_force_condition, METH_VARARGS},
                                        {"set_compliant_type_enable", (PyCFunction)set_compliant_type_enable, METH_VARARGS},
                                        {"setup_io_channel", (PyCFunction)setup_io_channel, METH_VARARGS},
                                        {"set_br_start_vol", (PyCFunction)set_br_start_vol, METH_VARARGS},
                                        {"shut_down", (PyCFunction)shut_down, METH_NOARGS},
                                        {"generate_ngc_file", (PyCFunction)generate_ngc_file, METH_VARARGS},
                                        {"traj_sample", (PyCFunction)traj_sample, METH_VARARGS},
                                        {"get_traj_sample_status", (PyCFunction)get_traj_sample_status, METH_VARARGS},
                                        {"set_traj_sample_data_filename", (PyCFunction)set_traj_sample_data_filename, METH_VARARGS},
                                        {"tio_set_pin_mode", (PyCFunction)tio_set_pin_mode, METH_VARARGS},
                                        {"tio_set_rs485_chn_mode", (PyCFunction)tio_set_rs485_chn_mode, METH_VARARGS},
                                        {"tio_set_rs485_chn_comm", (PyCFunction)tio_set_rs485_chn_comm, METH_VARARGS},
                                        {"tio_set_vout_ena", (PyCFunction)tio_set_vout_ena, METH_VARARGS},
                                        {"tio_set_vout_vol", (PyCFunction)tio_set_vout_vol, METH_VARARGS},
                                        {"tio_add_rs_signal", (PyCFunction)tio_add_rs_signal, METH_VARARGS},
                                        {"tio_del_rs_signal", (PyCFunction)tio_del_rs_signal, METH_VARARGS},
                                        {"tio_update_rs_signal", (PyCFunction)tio_update_rs_signal, METH_VARARGS},
                                        {"tio_send_rs_command", (PyCFunction)tio_send_rs_command, METH_VARARGS},
                                        {"set_capture", (PyCFunction)set_capture, METH_VARARGS},
                                        {"clear_capture", (PyCFunction)clear_capture, METH_VARARGS},
                                        {"set_servoj_filter", (PyCFunction)set_servoj_filter, METH_VARARGS},
                                        {"set_robot_safety_settings", (PyCFunction)set_robot_safety_settings, METH_VARARGS},
                                        {"set_7_dof_inv_inform", (PyCFunction)set_7_dof_inv_inform, METH_VARARGS},
                                        {"get_robot_safety_settings", (PyCFunction)get_robot_safety_settings, METH_VARARGS},
                                        {"set_reduce_safety_settings", (PyCFunction)set_reduce_safety_settings, METH_VARARGS},
                                        {"set_drag_speed_limit_settings", (PyCFunction)set_drag_speed_limit_settings, METH_VARARGS},
                                        {"set_joint_safety_settings", (PyCFunction)set_joint_safety_settings, METH_VARARGS},
                                        {"get_joint_safety_settings", (PyCFunction)get_joint_safety_settings, METH_VARARGS},
                                        {"reset_step_status", (PyCFunction)reset_step_status, METH_VARARGS},
                                        {"set_network_exception_handle", (PyCFunction)set_network_exception_handle, METH_VARARGS},
                                        {"set_collision_option", (PyCFunction)set_collision_option, METH_VARARGS},
                                        {"config_profinet", (PyCFunction)config_profinet, METH_VARARGS},
                                        {"clear_profinet_funcio", (PyCFunction)clear_profinet_funcio, METH_VARARGS},
                                        {"set_eip", (PyCFunction)Set_eip, METH_VARARGS},
                                        {"set_grpc_errcode", (PyCFunction)set_grpc_errcode, METH_VARARGS},
                                        // {"get_grpc_errcode", (PyCFunction)get_grpc_errcode, METH_VARARGS},
                                        // {"set_backup_errcode", (PyCFunction)set_backup_errcode, METH_VARARGS},
                                        // {"get_backup_errcode", (PyCFunction)get_backup_errcode, METH_VARARGS},
                                        {"set_free_button_opt", (PyCFunction)set_free_button_opt, METH_VARARGS},
                                        {"set_free_prog_button_opt", (PyCFunction)set_free_prog_button_opt, METH_VARARGS},
                                        {"set_point_button_opt", (PyCFunction)set_point_button_opt, METH_VARARGS},
                                        {"set_collision_gm_record_enable", (PyCFunction)set_collision_gm_record_enable, METH_VARARGS},
                                        {"set_home_vel", (PyCFunction)set_home_vel, METH_VARARGS},
                                        {"stop_identify_friction", (PyCFunction)stop_identify_friction, METH_VARARGS},
                                        {"start_identify_friction", (PyCFunction)start_identify_friction, METH_VARARGS},
                                        {"set_identify_friction_end_pos", (PyCFunction)set_identify_friction_end_pos, METH_VARARGS},
                                        {"set_identify_friction_start_pos", (PyCFunction)set_identify_friction_start_pos, METH_VARARGS},
                                        {"set_servo_dynamics_para", (PyCFunction)set_servo_dynamics_para, METH_VARARGS},
                                        {"set_servo_friction", (PyCFunction)set_servo_friction, METH_VARARGS},
                                        {"set_force_limit_value", (PyCFunction)set_force_limit_value, METH_VARARGS},
                                        {"set_collision_detection_enable", (PyCFunction)set_collision_detection_enable, METH_VARARGS},
                                        {"get_RS485_message", (PyCFunction)get_RS485_message, METH_VARARGS},
                                        {"set_TIO_RS485_sig_prog_stat", (PyCFunction)set_TIO_RS485_sig_prog_stat, METH_VARARGS},
                                        {"set_identify_cycle_time", (PyCFunction)set_identify_cycle_time, METH_VARARGS},
                                        {"start_identify_dynamics", (PyCFunction)start_identify_dynamics, METH_VARARGS},
                                        {"stop_identify_dynamics", (PyCFunction)stop_identify_dynamics, METH_VARARGS},
                                        {"set_collision_sensitivity", (PyCFunction)set_collision_sensitivity, METH_VARARGS},
                                        {"set_backdrive_on", (PyCFunction)set_backdrive_on, METH_VARARGS},
                                        {"set_sim_enable", (PyCFunction)set_sim_enable, METH_VARARGS},
                                        {"set_sim_robot", (PyCFunction)set_sim_robot, METH_VARARGS},
                                        {"sync_zucsettings", (PyCFunction)sync_zucsettings, METH_VARARGS},
                                        {"diag_collect_data", (PyCFunction)diag_collect_data, METH_VARARGS},
                                        {"axis_power", (PyCFunction)axis_power, METH_VARARGS},
                                        {"axis_reset", (PyCFunction)axis_reset, METH_VARARGS},
                                        {"set_7_dof_avoid_limit_param", (PyCFunction)set_7_dof_avoid_limit_param_alpha_k, METH_VARARGS},
                                        {"set_full_dh_flag", (PyCFunction)set_full_dh_flag, METH_VARARGS},
                                        {"set_torque_control_enable", (PyCFunction)set_torque_control_enable, METH_VARARGS},
                                        {"set_torque_control_value", (PyCFunction)set_torque_control_value, METH_VARARGS},
                                        {"set_gravity_direction", (PyCFunction)set_gravity_direction, METH_VARARGS},
                                        {"set_drag_control_enable", (PyCFunction)set_drag_control_enable, METH_VARARGS},
                                        {"set_drag_control_fc_compensate", (PyCFunction)set_drag_control_fc_compensate, METH_VARARGS},
                                        {"set_drag_control_fv_compensate", (PyCFunction)set_drag_control_fv_compensate, METH_VARARGS},
                                        {"set_drag_control_filter_factor", (PyCFunction)set_drag_control_filter_factor, METH_VARARGS},
                                        {"set_drag_control_dither_factor", (PyCFunction)set_drag_control_dither_factor, METH_VARARGS},
                                        {"robot_jog", (PyCFunction)robot_jog, METH_VARARGS},
                                        {"robot_jog_multi_joint", (PyCFunction)robot_jog_multi_joint, METH_VARARGS},
                                        {"robot_set_drag_enable", (PyCFunction)robot_set_drag_enable, METH_VARARGS},
                                        {"robot_set_payload_id", (PyCFunction)robot_set_payload_id, METH_VARARGS},
                                        {"robot_servo_home", (PyCFunction)robot_servo_home, METH_VARARGS},
                                        {"robot_clear_error", (PyCFunction)robot_clear_error, METH_VARARGS},
                                        {"robot_motion_abort", (PyCFunction)robot_motion_abort, METH_VARARGS},
                                        {"robot_set_base_offset", (PyCFunction)robot_set_base_offset, METH_VARARGS},
                                        {NULL}};

static PyTypeObject Command_Type = {
    .ob_base = PyVarObject_HEAD_INIT(NULL, 0).tp_name = "jkzuc.command", /*tp_name*/
    .tp_basicsize = sizeof(pyCommandChannel),                            /*tp_basicsize*/
    /* methods */
    .tp_dealloc = (destructor)Command_dealloc, /*tp_dealloc*/
    .tp_flags = Py_TPFLAGS_DEFAULT,            /*tp_flags*/
    .tp_methods = Command_methods,             /*tp_methods*/
    .tp_getset = Command_getset,               /*tp_members*/
    .tp_init = (initproc)Command_init,         /*tp_init*/
    .tp_new = PyType_GenericNew,               /*tp_new*/
};

// 初始化Command的类型, 返回0表示成功，其它表示失败
int32_t Cmd_Ready() { return PyType_Ready(&Command_Type); }

// 初始化Command对象，成功返回对象指针，其它为NULL
PyObject* Cmd_CreateObject()
{
    PyObject* cmd = (PyObject*)&Command_Type;
    Py_XINCREF(cmd);
    return cmd;
}

// 释放Command对象
int32_t Cmd_DestroyObject(PyObject* cmd)
{
    Py_XDECREF((PyObject*)&Command_Type);
    return 0;
}
